void make_rots(float **xmat, float rot_x, float rot_y, float rot_z)
{
float
**TRX,
**TRY,
**TRZ,
**T1;
ALLOC2D(TRX ,5,5);
ALLOC2D(TRY ,5,5);
ALLOC2D(TRZ ,5,5);
ALLOC2D(T1 ,5,5);
nr_rotxf(TRX, rot_x); /* create the rotate X matrix */
nr_rotyf(TRY, rot_y); /* create the rotate Y matrix */
nr_rotzf(TRZ, rot_z); /* create the rotate Z matrix */
nr_multf(TRY,1,4,1,4, TRX,1,4,1,4, T1); /* apply rx and ry */
nr_multf(TRZ,1,4,1,4, T1,1,4,1,4, xmat); /* apply rz */
FREE2D(TRX);
FREE2D(TRY);
FREE2D(TRZ);
FREE2D(T1 );
}
/*
function getparams will get the trnsform parameters from a
transformation matrix 'tran' (that has already had the translation
componants removed).
in description below, I assume that tran is a forward xform, from
native space to talairach space.
I assume that trans is a 4x4 homogeneous matrix, with the
principal axis stored in the upper left 3x3 matrix. The first col
of tran represents is the coordinate of (1,0,0)' mapped through the
transformation. (this means vec2 = tran * vec1).
trans = [scale][shear][rot]
= [scale][shear][rz][ry][rx];
the shear matrix is constrained to be of the form:
shear = [1 0 0 0
f 1 0 0
g h 1 0
0 0 0 1];
where f,g,h can take on any value.
the scale matrix is constrained to be of the form:
scale = [sx 0 0 0
0 sy 0 0
0 0 sz 0
0 0 0 1];
where sx,sy,sz must be positive.
all rotations are assumed to be in the range -pi/2..pi/2
the rotation angles are returned as radians and are applied
counter clockwise, when looking down the axis (from the positive end
towards the origin).
trans is assumed to be invertible.
i assume a coordinate system:
^ y
|
|
|
|_______> x
/
/
/z (towards the viewer).
procedure:
start with t = inv(tran) (after removing translation from tran )
t = inv(r)*inv(sh)*inv(sc)
t maps the talairach space unit vectors into native
space, which means that the columns of T are the
direction cosines of these vectors x,y,z
(1) the length of the vectors x,y,z give the inverse scaling
parameters:
sx = 1/norm(x); sy = 1/norm(y); sz = 1/norm(z);
(2) with the constraints on the form of sh above, inv(sh) has the
same form. let inv(sh) be as above in terms of a,b and c.
inv(sh) = [1 0 0 0; a 1 0 0; b c 1 0; 0 0 0 1];
for c: project y onto z and normalize:
/ |y.z|^2 \(1/2)
c = < --------------- >
\ |y|^2 - |y.z|^2 /
for b: project x onto z and normalize
/ |x.z|^2 \(1/2)
b = < --------------------- >
\ |x|^2 - |x.z|^2 - a^2 /
where a is the projection of x onto the coordinate sys Y axis.
for a: project x onto z and normalize
a is taken from (b) above, and normalized... see below
(3) rots are returned by getrots by giving the input transformation:
rot_mat = [inv(sh)][inv(sc)][trans]
(4) once completed, the parameters of sx,sy,sz and a,b,c are
adjusted so that they maintain the matrix contraints above.
*/
VIO_BOOL extract2_parameters_from_matrix(VIO_Transform *trans,
double *center,
double *translations,
double *scales,
double *shears,
double *rotations)
{
int
i,j;
float
n1,n2,
magnitude, magz, magx, magy, ai,bi,ci,scalar,a1,
**center_of_rotation,
**result,
**unit_vec,
*ang,*tmp,**x,**y,**z, **nz, **y_on_z, **ortho_y,
**xmat,**T,**Tinv,**C,**Sinv,
**R,**SR,**SRinv,**Cinv,**TMP1,**TMP2;
ALLOC2D(xmat ,5,5); nr_identf(xmat ,1,4,1,4);
ALLOC2D(TMP1 ,5,5); nr_identf(TMP1 ,1,4,1,4);
ALLOC2D(TMP2 ,5,5); nr_identf(TMP2 ,1,4,1,4);
ALLOC2D(Cinv ,5,5); nr_identf(Cinv ,1,4,1,4);
ALLOC2D(SR ,5,5); nr_identf(SR ,1,4,1,4);
ALLOC2D(SRinv ,5,5); nr_identf(SRinv,1,4,1,4);
ALLOC2D(Sinv ,5,5); nr_identf(Sinv ,1,4,1,4);
ALLOC2D(T ,5,5); nr_identf(T ,1,4,1,4);
ALLOC2D(Tinv ,5,5); nr_identf(Tinv ,1,4,1,4);
ALLOC2D(C ,5,5); nr_identf(C ,1,4,1,4);
ALLOC2D(R ,5,5); nr_identf(R ,1,4,1,4);
ALLOC2D(center_of_rotation ,5,5); /* make column vectors */
ALLOC2D(result ,5,5);
ALLOC2D(unit_vec ,5,5);
ALLOC2D(x ,5,5);
ALLOC2D(y ,5,5);
ALLOC2D(z ,5,5);
ALLOC2D(nz ,5,5);
ALLOC2D(y_on_z ,5,5);
ALLOC2D(ortho_y ,5,5);
ALLOC(tmp ,4);
ALLOC(ang ,4);
for(i=0; i<=3; i++) /* copy the input matrix */
for(j=0; j<=3; j++)
xmat[i+1][j+1] = (float)Transform_elem(*trans,i,j);
/* -------------DETERMINE THE TRANSLATION FIRST! --------- */
/* see where the center of rotation is displaced... */
FILL_NR_COLVEC( center_of_rotation, center[0], center[1], center[2] );
invertmatrix(4, xmat, TMP1); /* get inverse of the matrix */
matrix_multiply( 4, 4, 1, xmat, center_of_rotation, result); /* was TMP! in place of xmat */
SUB_NR_COLVEC( result, result, center_of_rotation );
for(i=0; i<=2; i++)
translations[i] = result[i+1][1];
/* -------------NOW GET THE SCALING VALUES! ----------------- */
for(i=0; i<=2; i++)
tmp[i+1] = -translations[i];
translation_to_homogeneous(3, tmp, Tinv);
for(i=0; i<=2; i++)
tmp[i+1] = center[i];
translation_to_homogeneous(3, tmp, C);
for(i=0; i<=2; i++)
tmp[i+1] = -center[i];
translation_to_homogeneous(3, tmp, Cinv);
matrix_multiply(4,4,4, xmat, C, TMP1); /* get scaling*shear*rotation matrix */
matrix_multiply(4,4,4, Tinv, TMP1, TMP1);
matrix_multiply(4,4,4, Cinv, TMP1, SR);
invertmatrix(4, SR, SRinv); /* get inverse of scaling*shear*rotation */
/* find each scale by mapping a unit vector backwards,
and finding the magnitude of the result. */
FILL_NR_COLVEC( unit_vec, 1.0, 0.0, 0.0 );
matrix_multiply( 4, 4, 1, SRinv, unit_vec, result);
magnitude = MAG_NR_COLVEC( result );
if (magnitude != 0.0) {
scales[0] = 1/magnitude;
Sinv[1][1] = magnitude;
}
else {
scales[0] = 1.0;
Sinv[1][1] = 1.0;
}
FILL_NR_COLVEC( unit_vec, 0.0, 1.0, 0.0 );
matrix_multiply( 4, 4, 1, SRinv, unit_vec, result);
magnitude = MAG_NR_COLVEC( result );
if (magnitude != 0.0) {
scales[1] = 1/magnitude;
Sinv[2][2] = magnitude;
}
else {
scales[1] = 1.0;
Sinv[2][2] = 1.0;
}
FILL_NR_COLVEC( unit_vec, 0.0, 0.0, 1.0 );
matrix_multiply( 4, 4, 1, SRinv, unit_vec, result);
magnitude = MAG_NR_COLVEC( result );
if (magnitude != 0.0) {
scales[2] = 1/magnitude;
Sinv[3][3] = magnitude;
}
else {
scales[2] = 1.0;
Sinv[3][3] = 1.0;
}
/* ------------NOW GET THE SHEARS, using the info from above ----- */
/* SR contains the [scale][shear][rot], must multiply [inv(scale)]*SR
to get shear*rot. */
/* make [scale][rot] */
matrix_multiply(4,4, 4, Sinv, SR, TMP1);
/* get inverse of [scale][rot] */
invertmatrix(4, TMP1, SRinv);
FILL_NR_COLVEC(x, SRinv[1][1], SRinv[2][1], SRinv[3][1]);
FILL_NR_COLVEC(y, SRinv[1][2], SRinv[2][2], SRinv[3][2]);
FILL_NR_COLVEC(z, SRinv[1][3], SRinv[2][3], SRinv[3][3]);
/* get normalized z direction */
magz = MAG_NR_COLVEC(z);
SCALAR_MULT_NR_COLVEC( nz, z, 1/magz );
/* get a direction perpendicular
to z, in the yz plane. */
scalar = DOTSUM_NR_COLVEC( y, nz );
SCALAR_MULT_NR_COLVEC( y_on_z, nz, scalar );
SUB_NR_COLVEC( result, y, y_on_z ); /* result = y - y_on_z */
scalar = MAG_NR_COLVEC( result); /* ortho_y = result ./ norm(result) */
SCALAR_MULT_NR_COLVEC( ortho_y, result, 1/scalar);
/* GET C for the skew matrix */
scalar = DOTSUM_NR_COLVEC( y, nz ); /* project y onto z */
magy = MAG_NR_COLVEC(y);
ci = scalar / sqrt((double)( magy*magy - scalar*scalar)) ;
/* GET B for the skew matrix */
/* first need a1 */
a1 = DOTSUM_NR_COLVEC( x, ortho_y ); /* project x onto ortho_y */
magx = MAG_NR_COLVEC(x);
/* now get B */
scalar = DOTSUM_NR_COLVEC( x, nz );
bi = scalar / sqrt((double)( magx*magx - scalar*scalar - a1*a1)) ;
/* GET A for skew matrix */
ai = a1 / sqrt((double)( magx*magx - scalar*scalar - a1*a1));
/* normalize the inverse shear parameters.
so that there is no scaling in the matrix
(all scaling is already accounted for
in sx,sy and sz. */
n1 = sqrt((double)(1 + ai*ai + bi*bi));
n2 = sqrt((double)(1 + ci*ci));
ai = ai / n1;
bi = bi / n1;
ci = ci / n2;
/* ai,bi,c1 now contain the parameters for
the inverse NORMALIZED shear matrix
(i.e., norm(col_i) = 1.0). */
/* ------------NOW GET THE ROTATION ANGLES!----- */
/* since SR = [scale][shear][rot], then
rot = [inv(shear)][inv(scale)][SR] */
nr_identf(TMP1 ,1,4,1,4); /* make inverse scale matrix */
TMP1[1][1] = 1/scales[0];
TMP1[2][2] = 1/scales[1];
TMP1[3][3] = 1/scales[2];
nr_identf(TMP2 ,1,4,1,4); /* make_inverse normalized shear matrix */
TMP2[1][1] = sqrt((double)(1 - ai*ai - bi*bi));
TMP2[2][2] = sqrt((double)(1 - ci*ci));
TMP2[2][1] = ai;
TMP2[3][1] = bi;
TMP2[3][2] = ci;
/* extract rotation matrix */
matrix_multiply(4,4, 4, TMP2, TMP1, T);
matrix_multiply(4,4, 4, T, SR, R);
/* get rotation angles */
if (!rotmat_to_ang(R, ang)) {
(void)fprintf(stderr,"Cannot convert R to radians!");
printmatrix(3,3,R);
return(FALSE);
}
for(i=0; i<=2; i++)
rotations[i] = ang[i+1];
/* ------------NOW ADJUST THE SCALE AND SKEW PARAMETERS ------------ */
invertmatrix(4, T, Tinv); /* get inverse of the matrix */
scales[0] = Tinv[1][1];
scales[1] = Tinv[2][2];
scales[2] = Tinv[3][3];
shears[0] = Tinv[2][1]/scales[1] ;
shears[1] = Tinv[3][1]/scales[2] ;
shears[2] = Tinv[3][2]/scales[2] ;
FREE2D(xmat);
FREE2D(TMP1);
FREE2D(TMP2);
FREE2D(Cinv);
FREE2D(SR );
FREE2D(SRinv);
FREE2D(Sinv);
FREE2D(T );
FREE2D(Tinv);
FREE2D(C );
FREE2D(R );
FREE2D(center_of_rotation);
FREE2D(result );
FREE2D(unit_vec );
FREE2D(x );
FREE2D(y );
FREE2D(z );
FREE2D(nz );
FREE2D(y_on_z );
FREE2D(ortho_y );
FREE(ang);
FREE(tmp);
return(TRUE);
}
VIO_BOOL extract_parameters_from_matrix(VIO_Transform *trans,
double *center,
double *translations,
double *scales,
double *shears,
double *rotations)
{
int
i,j;
float
magnitude,
**center_of_rotation,
**result,
**unit_vec,
*ang,*tmp,
**xmat,**T,**Tinv,**C,**Sinv,**R,**SR,**SRinv,**Cinv,**TMP1,**TMP2;
ALLOC2D(xmat ,5,5); nr_identf(xmat ,1,4,1,4);
ALLOC2D(TMP1 ,5,5); nr_identf(TMP1 ,1,4,1,4);
ALLOC2D(TMP2 ,5,5); nr_identf(TMP2 ,1,4,1,4);
ALLOC2D(Cinv ,5,5); nr_identf(Cinv ,1,4,1,4);
ALLOC2D(SR ,5,5); nr_identf(SR ,1,4,1,4);
ALLOC2D(SRinv ,5,5); nr_identf(SRinv,1,4,1,4);
ALLOC2D(Sinv ,5,5); nr_identf(Sinv ,1,4,1,4);
ALLOC2D(T ,5,5); nr_identf(T ,1,4,1,4);
ALLOC2D(Tinv ,5,5); nr_identf(Tinv ,1,4,1,4);
ALLOC2D(C ,5,5); nr_identf(C ,1,4,1,4);
ALLOC2D(R ,5,5); nr_identf(R ,1,4,1,4);
ALLOC2D(center_of_rotation ,5,5); /* make column vectors */
ALLOC2D(result ,5,5);
ALLOC2D(unit_vec ,5,5);
ALLOC(tmp ,4);
ALLOC(ang ,4);
for(i=0; i<=3; i++) /* copy the input matrix */
for(j=0; j<=3; j++)
xmat[i+1][j+1] = (float)Transform_elem(*trans,i,j);
/* -------------DETERMINE THE TRANSLATION FIRST! --------- */
/* see where the center of rotation is displaced... */
FILL_NR_COLVEC( center_of_rotation, center[0], center[1], center[2] );
invertmatrix(4, xmat, TMP1); /* get inverse of the matrix */
matrix_multiply( 4, 4, 1, xmat, center_of_rotation, result); /* was TMP! in place of xmat */
SUB_NR_COLVEC( result, result, center_of_rotation );
for(i=0; i<=2; i++)
translations[i] = result[i+1][1];
/* -------------NOW GET THE SCALING VALUES! ----------------- */
for(i=0; i<=2; i++)
tmp[i+1] = -translations[i];
translation_to_homogeneous(3, tmp, Tinv);
for(i=0; i<=2; i++)
tmp[i+1] = center[i];
translation_to_homogeneous(3, tmp, C);
for(i=0; i<=2; i++)
tmp[i+1] = -center[i];
translation_to_homogeneous(3, tmp, Cinv);
matrix_multiply(4,4,4, xmat, C, TMP1); /* get scaling*rotation matrix */
matrix_multiply(4,4,4, Tinv, TMP1, TMP1);
matrix_multiply(4,4,4, Cinv, TMP1, SR);
invertmatrix(4, SR, SRinv); /* get inverse of scaling*rotation */
/* find each scale by mapping a unit vector backwards,
and finding the magnitude of the result. */
FILL_NR_COLVEC( unit_vec, 1.0, 0.0, 0.0 );
matrix_multiply( 4, 4, 1, SRinv, unit_vec, result);
magnitude = MAG_NR_COLVEC( result );
if (magnitude != 0.0) {
scales[0] = 1/magnitude;
Sinv[1][1] = magnitude;
}
else {
scales[0] = 1.0;
Sinv[1][1] = 1.0;
}
FILL_NR_COLVEC( unit_vec, 0.0, 1.0, 0.0 );
matrix_multiply( 4, 4, 1, SRinv, unit_vec, result);
magnitude = MAG_NR_COLVEC( result );
if (magnitude != 0.0) {
scales[1] = 1/magnitude;
Sinv[2][2] = magnitude;
}
else {
scales[1] = 1.0;
Sinv[2][2] = 1.0;
}
FILL_NR_COLVEC( unit_vec, 0.0, 0.0, 1.0 );
matrix_multiply( 4, 4, 1, SRinv, unit_vec, result);
magnitude = MAG_NR_COLVEC( result );
if (magnitude != 0.0) {
scales[2] = 1/magnitude;
Sinv[3][3] = magnitude;
}
else {
scales[2] = 1.0;
Sinv[3][3] = 1.0;
}
/* ------------NOW GET THE ROTATION ANGLES!----- */
/* extract rotation matrix */
matrix_multiply(4,4, 4, Sinv, SR, R);
/* get rotation angles */
if (!rotmat_to_ang(R, ang)) {
(void)fprintf(stderr,"Cannot convert R to radians!");
printmatrix(3,3,R);
return(FALSE);
}
for(i=0; i<=2; i++)
rotations[i] = ang[i+1];
FREE2D(xmat);
FREE2D(TMP1);
FREE2D(TMP2);
FREE2D(Cinv);
FREE2D(SR );
FREE2D(SRinv);
FREE2D(Sinv);
FREE2D(T );
FREE2D(Tinv);
FREE2D(C );
FREE2D(R );
FREE2D(center_of_rotation);
FREE2D(result );
FREE2D(unit_vec );
FREE(ang);
FREE(tmp);
return(TRUE);
}
/* ----------------------------- MNI Header -----------------------------------
@NAME : build_inverse_transformation_matrix_quater
@INPUT : center, translations, scales, quaternions
@OUTPUT : the inverse linear transformation matrix of mat:
since mat = (T)(C)(SH)(S)(R)(-C), then
invmat = (C)(inv(r))(inv(S))(inv(SH))(-C)(-T)
@RETURNS : nothing
@DESCRIPTION:
the matrix is to be PREmultiplied with a vector (mat*vec)
when used in the application
same as build_inverse_transformation_matrix but with quaternions
@METHOD :
@GLOBALS :
@CALLS :
@CREATED : Thr Apr 18 10:45:56 EST 2002 pln
@MODIFIED :
---------------------------------------------------------------------------- */
void build_inverse_transformation_matrix_quater(VIO_Transform *trans,
double *center,
double *translations,
double *scales,
double *shears,
double *quaternions)
{
float
**T,
**SH,
**S,
**R,
**C,
**T1,
**T2,
**T3,
**T4;
int
i,j;
ALLOC2D(T ,5,5);
ALLOC2D(SH ,5,5);
ALLOC2D(S ,5,5);
ALLOC2D(R ,5,5);
ALLOC2D(C ,5,5);
ALLOC2D(T1 ,5,5);
ALLOC2D(T2 ,5,5);
ALLOC2D(T3 ,5,5);
ALLOC2D(T4 ,5,5);
/* invmat = (C)(inv(r))(inv(S))(inv(SH))(-C)(-T)
mat = (T)(C)(SH)(S)(R)(-C) */
nr_identf(T,1,4,1,4); /* make (-T)(-C) */
for(i=0; i<3; i++) {
T[1+i][4] = -translations[i] - center[i];
}
build_rotmatrix(T1,quaternions); /* make rotation matrix from quaternions */
transpose(4,4,T1,R);
make_shears(T1,shears); /* make shear rotation matrix */
invertmatrix(4, T1, SH); /* get inverse of the matrix */
/* make scaling matrix */
nr_identf(S,1,4,1,4);
for(i=0; i<3; i++) {
if (scales[i] != 0.0)
S[1+i][1+i] = 1/scales[i];
else
S[1+i][1+i] = 1.0;
}
nr_identf(C,1,4,1,4); /* make center */
for(i=0; i<3; i++) {
C[1+i][4] = center[i];
}
nr_multf(C,1,4,1,4, R ,1,4,1,4, T1 );
nr_multf(T1,1,4,1,4, SH,1,4,1,4, T2 );
nr_multf(T2,1,4,1,4, S ,1,4,1,4, T3 );
nr_multf(T3,1,4,1,4, T ,1,4,1,4, T4 );
for(i=0; i<4; i++)
for(j=0; j<4; j++)
Transform_elem(*trans, i, j ) = T4[i+1][j+1];
FREE2D(T );
FREE2D(SH );
FREE2D(S );
FREE2D(R );
FREE2D(C );
FREE2D(T1 );
FREE2D(T2 );
FREE2D(T3 );
FREE2D(T4 );
}
void build_transformation_matrix_quater(VIO_Transform *trans,
double *center,
double *translations,
double *scales,
double *shears,
double *quaternions)
{
float
**T,
**SH,
**S,
**R,
**C,
**T1,
**T2,
**T3,
**T4;
double normal;
int
i,j;
ALLOC2D(T ,5,5);
ALLOC2D(SH ,5,5);
ALLOC2D(S ,5,5);
ALLOC2D(R ,5,5);
ALLOC2D(C ,5,5);
ALLOC2D(T1 ,5,5);
ALLOC2D(T2 ,5,5);
ALLOC2D(T3 ,5,5);
ALLOC2D(T4 ,5,5);
normal=(quaternions[0]*quaternions[0] + quaternions[1]*quaternions[1] + quaternions[2]*quaternions[2] + quaternions[3]*quaternions[3]);
if (normal>1){
for(i = 0; i < 4; i++){
quaternions[i] /= normal;
}}
/* mat = (T)(C)(SH)(S)(R)(-C) */
nr_identf(T,1,4,1,4);
/* make (T)(C) */
for(i=0; i<3; i++) {
T[1+i][4] = translations[i] + center[i];
}
build_rotmatrix(R,quaternions); /* make rotation matrix from quaternions */
/* make shear rotation matrix */
make_shears(SH, shears);
/* make scaling matrix */
nr_identf(S,1,4,1,4);
for(i=0; i<3; i++) {
S[1+i][1+i] = scales[i];
}
nr_identf(C,1,4,1,4); /* make center */
for(i=0; i<3; i++) {
C[1+i][4] = -center[i];
}
nr_multf(T, 1,4,1,4, S ,1,4,1,4, T1 );
nr_multf(T1,1,4,1,4, SH ,1,4,1,4, T2 );
nr_multf(T2,1,4,1,4, R ,1,4,1,4, T3 );
nr_multf(T3,1,4,1,4, C ,1,4,1,4, T4 );
for(i=0; i<4; i++)
for(j=0; j<4; j++)
Transform_elem(*trans, i, j ) = T4[i+1][j+1];
FREE2D(T );
FREE2D(SH );
FREE2D(S );
FREE2D(R );
FREE2D(C );
FREE2D(T1 );
FREE2D(T2 );
FREE2D(T3 );
FREE2D(T4 );
}
void build_transformation_matrix(VIO_Transform *trans,
double *center,
double *translations,
double *scales,
double *shears,
double *rotations)
{
float
**T,
**SH,
**S,
**R,
**C,
**T1,
**T2,
**T3,
**T4;
int
i,j;
ALLOC2D(T ,5,5);
ALLOC2D(SH ,5,5);
ALLOC2D(S ,5,5);
ALLOC2D(R ,5,5);
ALLOC2D(C ,5,5);
ALLOC2D(T1 ,5,5);
ALLOC2D(T2 ,5,5);
ALLOC2D(T3 ,5,5);
ALLOC2D(T4 ,5,5);
/* mat = (T)(C)(SH)(S)(R)(-C) */
nr_identf(T,1,4,1,4); /* make (T)(C) */
for(i=0; i<3; i++) {
T[1+i][4] = translations[i] + center[i];
}
/* make rotation matix */
make_rots(R,
(float)(rotations[0]),
(float)(rotations[1]),
(float)(rotations[2]));
/* make shear rotation matrix */
make_shears(SH, shears);
/* make scaling matrix */
nr_identf(S,1,4,1,4);
for(i=0; i<3; i++) {
S[1+i][1+i] = scales[i];
}
nr_identf(C,1,4,1,4); /* make center */
for(i=0; i<3; i++) {
C[1+i][4] = -center[i];
}
nr_multf(T, 1,4,1,4, S ,1,4,1,4, T1 );
nr_multf(T1,1,4,1,4, SH ,1,4,1,4, T2 );
nr_multf(T2,1,4,1,4, R ,1,4,1,4, T3 );
nr_multf(T3,1,4,1,4, C ,1,4,1,4, T4 );
for(i=0; i<4; i++)
for(j=0; j<4; j++)
Transform_elem(*trans, i, j ) = T4[i+1][j+1];
FREE2D(T );
FREE2D(SH );
FREE2D(S );
FREE2D(R );
FREE2D(C );
FREE2D(T1 );
FREE2D(T2 );
FREE2D(T3 );
FREE2D(T4 );
}
int main()
{
//computes velocity due to time step file generated from Free Wake 2007 at
//multiple points
//Step 1 read in information about point and time step from matlab file
//Step 2 read in time step file for the specified time step
//Step 3 compute velocities at point P
//Step 4 create output file of velocites at point P
//program checks computation of the induced velocity in P due to a DVE
//
//these program allows to compute the velocity field that is induced by a
//given wing and the corresponding wake
//
// !!!ATTENTION!!!
//
// The output path is OUTPUT_PATH, which is defined in general.h!!
//
struct Points
{double x;
double y;
double z;
}Points[10000];
struct Vels
{double u;
double v;
double w;
}Vels[10000];
double P[3],w_ind[3],tempA[3];
int i,j,k; // loop counter
int plane,type;
int imax,jmax,kmax;
int wing,span,time; //more loop counters
int numpoints=0; //number of points
double xmin,xmax,ymin,ymax,zmin,zmax;
double xstep,ystep,zstep;
double tempS;
char ch; //generic character
// GENERAL info; //general info
DVE *surfaceDVE, **wakeDVE;
int timestep;
char iofile[125]; //input-output-file
FILE *fp,*fs;
//===================================================================//
//START Step 1
//read in information about point and time step from matlab file
//===================================================================//
//creates file name for file with point information
sprintf(iofile,"%s%s",OUTPUT_PATH,"pointinfo.txt");
// checks if input file exists
if ((fp = fopen(iofile, "r"))== NULL)
{
printf("File could not be opened, stupid:\n");
exit(1);
}
//opens input file
fp = fopen("./output/pointinfo.txt", "r");
//scan number of points
fscanf(fp,"%d ", &numpoints);
for (i=0;i<numpoints;i++){
fscanf(fp,"%d %lf %lf %lf", ×tep,&Points[i].x,&Points[i].y,&Points[i].z);
}
fclose(fp);
//printf("%d %lf %lf %lf\n", timestep,P[0],P[1],P[2]);
//===================================================================//
//END Step 1
//===================================================================//
//===================================================================//
// START Step 2
// set-up to read in timestep-file
//===================================================================//
//creates file name timestep##.txt ## is number of timestep
sprintf(iofile,"%s%s%d%s",OUTPUT_PATH,"timestep",timestep,".txt");
// checks if input file exists
if ((fp = fopen(iofile, "r"))== NULL)
{
printf("File could not be opened, stupid:\n");
exit(1);
}
//opens input file
fp = fopen(iofile, "r");
//===================================================================//
// reads in general info
//===================================================================//
//find the ':'-sign in input file before program version
do ch = fgetc(fp);
while (ch!=':');
//find the ':'-sign in input file before reference area
do ch = fgetc(fp);
while (ch!=':');
fscanf(fp,"%lf", &info.S);
//printf(" %lf\n",info.S);
//find the ':'-sign in input file before aspect ratio
do ch = fgetc(fp);
while (ch!=':');
fscanf(fp,"%lf", &info.AR);
//printf(" %lf",info.AR);
//find the ':'-sign in input file before alpha
do ch = fgetc(fp);
while (ch!=':');
fscanf(fp,"%lf", &info.alpha);
// info.alpha *= DtR;
//find the ':'-sign in input file before beta
do ch = fgetc(fp);
while (ch!=':');
fscanf(fp,"%lf", &info.beta);
info.beta *= DtR;
//find the ':'-sign in input file before symmetry condition
do ch = fgetc(fp);
while (ch!=':');
fscanf(fp,"%d", &info.sym);
//find the ':'-sign in input file before timestep
do ch = fgetc(fp);
while (ch!=':');
// //reads timestep
// fscanf(fp,"%*d", ×tep);
//find the ':'-sign in input file before number of elements along span
do ch = fgetc(fp);
while (ch!=':');
//reads nospanelements
fscanf(fp,"%d", &info.nospanelement);
//printf("%d\n", info.nospanelement);
//find the ':'-sign in input file before number of elements along chord
do ch = fgetc(fp);
while (ch!=':');
//reads nospanelements
fscanf(fp,"%d", &info.m);
//printf("%d\n", info.m);
//number of surface DVEs
info.noelement = info.m*info.nospanelement;
//find the ':'-sign in input file before number of wings
do ch = fgetc(fp);
while (ch!=':');
//reads nospanelements
fscanf(fp,"%d\n", &info.nowing);
//find the ':'-sign in input file before number of span indices
do ch = fgetc(fp);
while (ch!=':');
for(wing=0;wing<info.nowing;wing++)
{
fscanf(fp,"%d", &info.wing1[wing]);
fscanf(fp,"%d", &info.wing2[wing]);
}
fclose(fp);
//===================================================================//
// allocates memory
//===================================================================//
ALLOC1D(&surfaceDVE,info.noelement);
ALLOC2D(&wakeDVE,timestep+1,info.nospanelement);
//===================================================================//
// reads info surface info
//===================================================================//
//read in information on surface and wake DVEs of timestepfile
Read_Timestep(timestep,surfaceDVE,wakeDVE);
//Subroutine in read_input.cpp
//===================================================================//
//-- Reading in is DONE --
//Now the singularity factors are being computed
//===================================================================//
// computes singularity factors for wake
//loop over timesteps
for(time=0;time<=timestep;time++)
//loop over number of wings
for(wing=0;wing<info.nowing;wing++)
{
//is the wing symmetrical or not?
if(info.sym == 1) //decay factor is 1% of tip-element half-span
tempS = 0.01*wakeDVE[time][info.wing2[wing]].eta;
else//wing has two tips, possibly different in geometry
{ //in that case, decay factor is 1% of the shorter half-span
if( wakeDVE[time][info.wing1[wing]].eta
< wakeDVE[time][info.wing2[wing]].eta)
tempS = 0.01*wakeDVE[time][info.wing1[wing]].eta;
else tempS = 0.01*wakeDVE[time][info.wing2[wing]].eta;
}
//loop over wale DVEs of current timestep
for(span=info.wing1[wing];span<=info.wing2[wing];span++)
wakeDVE[time][span].singfct = tempS; //assigning decay factor
}//next wing
//singularity factor of lifting surface DVEs
//computing decaying factor for added singularity at wing tip
k=0; //initializing index counter of first DVE of wing
for(wing=0;wing<info.nowing;wing++)
{
//index of last DVE of this wing (located at tip and trail. edge)
span = k + (info.wing2[wing]-info.wing1[wing]+1)*info.m - 1;
//is the wing symmetrical or not?
if(info.sym == 1) //decay factor is 1% of tip-element half-span
tempS = 0.01*surfaceDVE[span].eta;
else//wing has two tips, possibly different in geometry
{ //in that case, decay factor is 1% of the shorter half-span
if( surfaceDVE[k].eta < surfaceDVE[span].eta)
tempS = 0.01*surfaceDVE[k].eta;
else tempS = 0.01*surfaceDVE[span].eta;
}
//loop over surface DVEs of current wing
for(i=k;i<=span;i++)
surfaceDVE[i].singfct = tempS; //assigning decay factor
k = span+1; //updating index for next wing
}//next wing
//===================================================================//
//-- Computation of singularity factors is DONE --
//Now comes the part where the velocity is computed
//===================================================================//
//===================================================================//
//END Step 2
//===================================================================//
//===================================================================//
//START Step 3
//computing velocity in P
//===================================================================//
i=0;
for(i=0;i<numpoints;i++)
{
P[0] = Points[i].x;
P[1] = Points[i].y;
P[2] = Points[i].z;
//velocity is induced by all surface and wake DVE's in point P
DVE_Induced_Velocity(info,P,surfaceDVE,wakeDVE,timestep,w_ind);
//subroutine in induced_velocity.cpp
Vels[i].u = w_ind[0];
Vels[i].v = w_ind[1];
Vels[i].w = w_ind[2];
}
//computing induced velocity of most right point of each wing.
//right wingtip points are stored in xright and the velocity in uright
//for(wing=0;wing<info.nowing;wing++)
//{
// for(time=0;time<=timestep;time++)
// {
//span index of DVE that is the most right on of current wing
// span = info.wing2[wing];
// if(time==0) type = 3;
// else if(time==timestep) type = 2;
// else type = 1;
// DVE_Tip_Induced_Velocity(info,wakeDVE[time][span],P,tempA,type);
//subroutine in induced_velocity.cpp
// w_ind[0] -= tempA[0];
// w_ind[1] -= tempA[1];
// w_ind[2] -= tempA[2];
// }
//}
//===================================================================//
//END Step 3
//===================================================================//
// START Step 4
// saving output (coordinates and induced velocity components)
//===================================================================//
//printf("\n Induced Velocities \n");
//printf("%lf %lf %lf\n",w_ind[0],w_ind[1],w_ind[2]);
// Create Output file
sprintf(iofile,"%s%s",OUTPUT_PATH,"velocityinfo.txt");
// checks if input file exists
// if ((fp = fopen(iofile, "r"))== NULL)
//{
// printf("File could not be opened, stupid:\n");
// exit(1);
//}
// Open output file and write w_ind
fs = fopen(iofile,"w");
fprintf(fs,"%d\n",numpoints);
i=0;
for(i=0;i<numpoints;i++)
{
fprintf(fs,"%1f %1f %1f\n", Vels[i].u,Vels[i].v,Vels[i].w);
}
fclose(fs);
// End Create Output file
// printf("\n DONE\n");
printf("\nIt's done. Enter anything, anything: ");
//scanf("%d",×tep);
return(0);
}
BICAPI BOOLEAN singular_value_decomposition(
int m,
int n,
Real **a,
Real w[],
Real **v )
{
int i,j;
BOOLEAN val;
char jobu = 'O';
char jobvt = 'A';
long int _m = (long int) m;
long int _n = (long int) n;
long int lda = _m;
long int ldu = _m;
long int ldvt = _n;
long int lwork = MAX(3*MIN(_m,_n)+MAX(_m,_n),5*MIN(_m,_n));
Real** _a;
Real* work;
Real** _u;
Real** _v;
long int info;
Real temp;
ALLOC(work,(int) lwork);
ALLOC2D(_a,n,m);
ALLOC2D(_u,n,m);
ALLOC2D(_v,n,n);
for (j = 0; j < n; j++) {
for (i = 0; i < m; i++) {
_a[j][i] = a[i][j];
}
}
val = bicpl_dgesvd_(&jobu, &jobvt, &_m, &_n, (Real*) *_a, &lda, (Real*) w,
(Real*) *_u, &ldu, (Real*) *_v, &ldvt,
(Real*) work, &lwork, &info);
for (j = 0; j < n; j++) {
for (i = 0; i < m; i++) {
a[i][j] = _a[j][i];
}
}
for (j = 0; j < n; j++) {
for (i = 0; i < n; i++) {
v[i][j] = _v[i][j];
}
}
FREE(work);
FREE2D(_u);
FREE2D(_v);
FREE2D(_a);
return val;
}