/*
* function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)
*/
void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T
P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T
eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
P_aposteriori[144])
{
real32_T wak[3];
real32_T O[9];
real_T dv0[9];
real32_T a[9];
int32_T i;
real32_T b_a[9];
real32_T x_n_b[3];
real32_T b_x_aposteriori_k[3];
real32_T z_n_b[3];
real32_T c_a[3];
real32_T d_a[3];
int32_T i0;
real32_T x_apriori[12];
real_T dv1[144];
real32_T A_lin[144];
static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
real32_T b_A_lin[144];
real32_T b_q[144];
real32_T c_A_lin[144];
real32_T d_A_lin[144];
real32_T e_A_lin[144];
int32_T i1;
real32_T P_apriori[144];
real32_T b_P_apriori[108];
static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1
};
real32_T K_k[108];
real32_T fv0[81];
static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1
};
real32_T b_r[81];
real32_T fv1[81];
real32_T f0;
real32_T c_P_apriori[36];
static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
real32_T fv2[36];
static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
real32_T c_r[9];
real32_T b_K_k[36];
real32_T d_P_apriori[72];
static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
1, 0, 0, 0
};
real32_T c_K_k[72];
static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
};
real32_T b_z[6];
static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1
};
static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 1
};
real32_T fv3[6];
real32_T c_z[6];
/* Extended Attitude Kalmanfilter */
/* */
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
/* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
/* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
/* */
/* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
/* */
/* Example.... */
/* */
/* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */
/* coder.varsize('udpIndVect', [9,1], [1,0]) */
/* udpIndVect=find(updVect); */
/* process and measurement noise covariance matrix */
/* Q = diag(q.^2*dt); */
/* observation matrix */
/* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */
/* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */
/* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */
/* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */
/* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */
/* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */
/* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */
/* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */
/* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */
/* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */
/* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */
/* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */
/* % prediction section */
/* body angular accelerations */
/* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */
wak[0] = x_aposteriori_k[3];
wak[1] = x_aposteriori_k[4];
wak[2] = x_aposteriori_k[5];
/* body angular rates */
/* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */
/* derivative of the prediction rotation matrix */
/* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
O[0] = 0.0F;
O[1] = -x_aposteriori_k[2];
O[2] = x_aposteriori_k[1];
O[3] = x_aposteriori_k[2];
O[4] = 0.0F;
O[5] = -x_aposteriori_k[0];
O[6] = -x_aposteriori_k[1];
O[7] = x_aposteriori_k[0];
O[8] = 0.0F;
/* prediction of the earth z vector */
/* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
eye(dv0);
for (i = 0; i < 9; i++) {
a[i] = (real32_T)dv0[i] + O[i] * dt;
}
/* prediction of the magnetic vector */
/* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
eye(dv0);
for (i = 0; i < 9; i++) {
b_a[i] = (real32_T)dv0[i] + O[i] * dt;
}
/* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */
/* 'attitudeKalmanfilter:66' -zez,0,zex; */
/* 'attitudeKalmanfilter:67' zey,-zex,0]'; */
/* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */
/* 'attitudeKalmanfilter:69' -muz,0,mux; */
/* 'attitudeKalmanfilter:70' zey,-mux,0]'; */
/* 'attitudeKalmanfilter:74' E=eye(3); */
/* 'attitudeKalmanfilter:76' Z=zeros(3); */
/* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */
x_n_b[0] = x_aposteriori_k[0];
x_n_b[1] = x_aposteriori_k[1];
x_n_b[2] = x_aposteriori_k[2];
b_x_aposteriori_k[0] = x_aposteriori_k[6];
b_x_aposteriori_k[1] = x_aposteriori_k[7];
b_x_aposteriori_k[2] = x_aposteriori_k[8];
z_n_b[0] = x_aposteriori_k[9];
z_n_b[1] = x_aposteriori_k[10];
z_n_b[2] = x_aposteriori_k[11];
for (i = 0; i < 3; i++) {
c_a[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];
}
d_a[i] = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];
}
x_apriori[i] = x_n_b[i] + dt * wak[i];
}
for (i = 0; i < 3; i++) {
x_apriori[i + 3] = wak[i];
}
for (i = 0; i < 3; i++) {
x_apriori[i + 6] = c_a[i];
}
for (i = 0; i < 3; i++) {
x_apriori[i + 9] = d_a[i];
}
/* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */
/* 'attitudeKalmanfilter:82' Z, Z, Z, Z */
/* 'attitudeKalmanfilter:83' EZ, Z, O, Z */
/* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */
/* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];
}
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * i) + 3] = 0.0F;
}
}
A_lin[6] = 0.0F;
A_lin[7] = x_aposteriori_k[8];
A_lin[8] = -x_aposteriori_k[7];
A_lin[18] = -x_aposteriori_k[8];
A_lin[19] = 0.0F;
A_lin[20] = x_aposteriori_k[6];
A_lin[30] = x_aposteriori_k[7];
A_lin[31] = -x_aposteriori_k[6];
A_lin[32] = 0.0F;
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;
}
}
A_lin[9] = 0.0F;
A_lin[10] = x_aposteriori_k[11];
A_lin[11] = -x_aposteriori_k[10];
A_lin[21] = -x_aposteriori_k[11];
A_lin[22] = 0.0F;
A_lin[23] = x_aposteriori_k[9];
A_lin[33] = x_aposteriori_k[7];
A_lin[34] = -x_aposteriori_k[9];
A_lin[35] = 0.0F;
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];
}
}
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *
dt;
}
}
/* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */
/* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */
/* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */
/* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */
/* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */
/* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */
/* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
b_q[0] = q[0];
b_q[12] = 0.0F;
b_q[24] = 0.0F;
b_q[36] = 0.0F;
b_q[48] = 0.0F;
b_q[60] = 0.0F;
b_q[72] = 0.0F;
b_q[84] = 0.0F;
b_q[96] = 0.0F;
b_q[108] = 0.0F;
b_q[120] = 0.0F;
b_q[132] = 0.0F;
b_q[1] = 0.0F;
b_q[13] = q[0];
b_q[25] = 0.0F;
b_q[37] = 0.0F;
b_q[49] = 0.0F;
b_q[61] = 0.0F;
b_q[73] = 0.0F;
b_q[85] = 0.0F;
b_q[97] = 0.0F;
b_q[109] = 0.0F;
b_q[121] = 0.0F;
b_q[133] = 0.0F;
b_q[2] = 0.0F;
b_q[14] = 0.0F;
b_q[26] = q[0];
b_q[38] = 0.0F;
b_q[50] = 0.0F;
b_q[62] = 0.0F;
b_q[74] = 0.0F;
b_q[86] = 0.0F;
b_q[98] = 0.0F;
b_q[110] = 0.0F;
b_q[122] = 0.0F;
b_q[134] = 0.0F;
b_q[3] = 0.0F;
b_q[15] = 0.0F;
b_q[27] = 0.0F;
b_q[39] = q[1];
b_q[51] = 0.0F;
b_q[63] = 0.0F;
b_q[75] = 0.0F;
b_q[87] = 0.0F;
b_q[99] = 0.0F;
b_q[111] = 0.0F;
b_q[123] = 0.0F;
b_q[135] = 0.0F;
b_q[4] = 0.0F;
b_q[16] = 0.0F;
b_q[28] = 0.0F;
b_q[40] = 0.0F;
b_q[52] = q[1];
b_q[64] = 0.0F;
b_q[76] = 0.0F;
b_q[88] = 0.0F;
b_q[100] = 0.0F;
b_q[112] = 0.0F;
b_q[124] = 0.0F;
b_q[136] = 0.0F;
b_q[5] = 0.0F;
b_q[17] = 0.0F;
b_q[29] = 0.0F;
b_q[41] = 0.0F;
b_q[53] = 0.0F;
b_q[65] = q[1];
b_q[77] = 0.0F;
b_q[89] = 0.0F;
b_q[101] = 0.0F;
b_q[113] = 0.0F;
b_q[125] = 0.0F;
b_q[137] = 0.0F;
b_q[6] = 0.0F;
b_q[18] = 0.0F;
b_q[30] = 0.0F;
b_q[42] = 0.0F;
b_q[54] = 0.0F;
b_q[66] = 0.0F;
b_q[78] = q[2];
b_q[90] = 0.0F;
b_q[102] = 0.0F;
b_q[114] = 0.0F;
b_q[126] = 0.0F;
b_q[138] = 0.0F;
b_q[7] = 0.0F;
b_q[19] = 0.0F;
b_q[31] = 0.0F;
b_q[43] = 0.0F;
b_q[55] = 0.0F;
b_q[67] = 0.0F;
b_q[79] = 0.0F;
b_q[91] = q[2];
b_q[103] = 0.0F;
b_q[115] = 0.0F;
b_q[127] = 0.0F;
b_q[139] = 0.0F;
b_q[8] = 0.0F;
b_q[20] = 0.0F;
b_q[32] = 0.0F;
b_q[44] = 0.0F;
b_q[56] = 0.0F;
b_q[68] = 0.0F;
b_q[80] = 0.0F;
b_q[92] = 0.0F;
b_q[104] = q[2];
b_q[116] = 0.0F;
b_q[128] = 0.0F;
b_q[140] = 0.0F;
b_q[9] = 0.0F;
b_q[21] = 0.0F;
b_q[33] = 0.0F;
b_q[45] = 0.0F;
b_q[57] = 0.0F;
b_q[69] = 0.0F;
b_q[81] = 0.0F;
b_q[93] = 0.0F;
b_q[105] = 0.0F;
b_q[117] = q[3];
b_q[129] = 0.0F;
b_q[141] = 0.0F;
b_q[10] = 0.0F;
b_q[22] = 0.0F;
b_q[34] = 0.0F;
b_q[46] = 0.0F;
b_q[58] = 0.0F;
b_q[70] = 0.0F;
b_q[82] = 0.0F;
b_q[94] = 0.0F;
b_q[106] = 0.0F;
b_q[118] = 0.0F;
b_q[130] = q[3];
b_q[142] = 0.0F;
b_q[11] = 0.0F;
b_q[23] = 0.0F;
b_q[35] = 0.0F;
b_q[47] = 0.0F;
b_q[59] = 0.0F;
b_q[71] = 0.0F;
b_q[83] = 0.0F;
b_q[95] = 0.0F;
b_q[107] = 0.0F;
b_q[119] = 0.0F;
b_q[131] = 0.0F;
b_q[143] = q[3];
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
A_lin[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
i0];
}
c_A_lin[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];
}
}
for (i0 = 0; i0 < 12; i0++) {
d_A_lin[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
}
e_A_lin[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
}
}
}
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];
}
}
/* % update */
/* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
/* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */
if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
/* 'attitudeKalmanfilter:112' r(2)=10000; */
r[1] = 10000.0F;
}
/* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */
/* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */
/* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */
/* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */
/* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */
/* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */
/* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */
/* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */
/* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */
/* observation matrix */
/* [zw;ze;zmk]; */
/* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */
/* 'attitudeKalmanfilter:126' Z, Z, E, Z; */
/* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */
/* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */
/* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */
/* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 9; i0++) {
b_P_apriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1
+ 12 * i0];
}
}
}
for (i = 0; i < 9; i++) {
for (i0 = 0; i0 < 12; i0++) {
K_k[i + 9 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
}
}
for (i0 = 0; i0 < 9; i0++) {
fv0[i + 9 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
}
}
}
b_r[0] = r[0];
b_r[9] = 0.0F;
b_r[18] = 0.0F;
b_r[27] = 0.0F;
b_r[36] = 0.0F;
b_r[45] = 0.0F;
b_r[54] = 0.0F;
b_r[63] = 0.0F;
b_r[72] = 0.0F;
b_r[1] = 0.0F;
b_r[10] = r[0];
b_r[19] = 0.0F;
b_r[28] = 0.0F;
b_r[37] = 0.0F;
b_r[46] = 0.0F;
b_r[55] = 0.0F;
b_r[64] = 0.0F;
b_r[73] = 0.0F;
b_r[2] = 0.0F;
b_r[11] = 0.0F;
b_r[20] = r[0];
b_r[29] = 0.0F;
b_r[38] = 0.0F;
b_r[47] = 0.0F;
b_r[56] = 0.0F;
b_r[65] = 0.0F;
b_r[74] = 0.0F;
b_r[3] = 0.0F;
b_r[12] = 0.0F;
b_r[21] = 0.0F;
b_r[30] = r[1];
b_r[39] = 0.0F;
b_r[48] = 0.0F;
b_r[57] = 0.0F;
b_r[66] = 0.0F;
b_r[75] = 0.0F;
b_r[4] = 0.0F;
b_r[13] = 0.0F;
b_r[22] = 0.0F;
b_r[31] = 0.0F;
b_r[40] = r[1];
b_r[49] = 0.0F;
b_r[58] = 0.0F;
b_r[67] = 0.0F;
b_r[76] = 0.0F;
b_r[5] = 0.0F;
b_r[14] = 0.0F;
b_r[23] = 0.0F;
b_r[32] = 0.0F;
b_r[41] = 0.0F;
b_r[50] = r[1];
b_r[59] = 0.0F;
b_r[68] = 0.0F;
b_r[77] = 0.0F;
b_r[6] = 0.0F;
b_r[15] = 0.0F;
b_r[24] = 0.0F;
b_r[33] = 0.0F;
b_r[42] = 0.0F;
b_r[51] = 0.0F;
b_r[60] = r[2];
b_r[69] = 0.0F;
b_r[78] = 0.0F;
b_r[7] = 0.0F;
b_r[16] = 0.0F;
b_r[25] = 0.0F;
b_r[34] = 0.0F;
b_r[43] = 0.0F;
b_r[52] = 0.0F;
b_r[61] = 0.0F;
b_r[70] = r[2];
b_r[79] = 0.0F;
b_r[8] = 0.0F;
b_r[17] = 0.0F;
b_r[26] = 0.0F;
b_r[35] = 0.0F;
b_r[44] = 0.0F;
b_r[53] = 0.0F;
b_r[62] = 0.0F;
b_r[71] = 0.0F;
b_r[80] = r[2];
for (i = 0; i < 9; i++) {
for (i0 = 0; i0 < 9; i0++) {
fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];
}
}
mrdivide(b_P_apriori, fv1, K_k);
/* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 9; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];
}
O[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 9; i0++) {
f0 += K_k[i + 12 * i0] * O[i0];
}
x_aposteriori[i] = x_apriori[i] + f0;
}
/* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 9; i1++) {
f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];
}
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12
* i0];
}
}
}
} else {
/* 'attitudeKalmanfilter:138' else */
/* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
/* 'attitudeKalmanfilter:141' R=[r(1),0,0; */
/* 'attitudeKalmanfilter:142' 0,r(1),0; */
/* 'attitudeKalmanfilter:143' 0,0,r(1)]; */
/* observation matrix */
/* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */
/* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
/* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 3; i0++) {
c_P_apriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
iv3[i1 + 12 * i0];
}
}
}
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 12; i0++) {
fv2[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *
i0];
}
}
for (i0 = 0; i0 < 3; i0++) {
O[i + 3 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];
}
}
}
c_r[0] = r[0];
c_r[3] = 0.0F;
c_r[6] = 0.0F;
c_r[1] = 0.0F;
c_r[4] = r[0];
c_r[7] = 0.0F;
c_r[2] = 0.0F;
c_r[5] = 0.0F;
c_r[8] = r[0];
for (i = 0; i < 3; i++) {
for (i0 = 0; i0 < 3; i0++) {
a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];
}
}
b_mrdivide(c_P_apriori, a, b_K_k);
/* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 3; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];
}
x_n_b[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 3; i0++) {
f0 += b_K_k[i + 12 * i0] * x_n_b[i0];
}
x_aposteriori[i] = x_apriori[i] + f0;
}
/* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 3; i1++) {
f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];
}
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +
12 * i0];
}
}
}
} else {
/* 'attitudeKalmanfilter:156' else */
/* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
{
/* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */
if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
/* 'attitudeKalmanfilter:159' r(2)=10000; */
r[1] = 10000.0F;
}
/* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */
/* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */
/* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */
/* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */
/* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */
/* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */
/* observation matrix */
/* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */
/* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */
/* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
/* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 6; i0++) {
d_P_apriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
iv5[i1 + 12 * i0];
}
}
}
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 12; i0++) {
c_K_k[i + 6 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12
* i0];
}
}
for (i0 = 0; i0 < 6; i0++) {
fv2[i + 6 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];
}
}
}
b_K_k[0] = r[0];
b_K_k[6] = 0.0F;
b_K_k[12] = 0.0F;
b_K_k[18] = 0.0F;
b_K_k[24] = 0.0F;
b_K_k[30] = 0.0F;
b_K_k[1] = 0.0F;
b_K_k[7] = r[0];
b_K_k[13] = 0.0F;
b_K_k[19] = 0.0F;
b_K_k[25] = 0.0F;
b_K_k[31] = 0.0F;
b_K_k[2] = 0.0F;
b_K_k[8] = 0.0F;
b_K_k[14] = r[0];
b_K_k[20] = 0.0F;
b_K_k[26] = 0.0F;
b_K_k[32] = 0.0F;
b_K_k[3] = 0.0F;
b_K_k[9] = 0.0F;
b_K_k[15] = 0.0F;
b_K_k[21] = r[1];
b_K_k[27] = 0.0F;
b_K_k[33] = 0.0F;
b_K_k[4] = 0.0F;
b_K_k[10] = 0.0F;
b_K_k[16] = 0.0F;
b_K_k[22] = 0.0F;
b_K_k[28] = r[1];
b_K_k[34] = 0.0F;
b_K_k[5] = 0.0F;
b_K_k[11] = 0.0F;
b_K_k[17] = 0.0F;
b_K_k[23] = 0.0F;
b_K_k[29] = 0.0F;
b_K_k[35] = r[1];
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 6; i0++) {
c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
}
}
c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
/* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 6; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
}
b_z[i] = z[i] - f0;
}
for (i = 0; i < 12; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 6; i0++) {
f0 += c_K_k[i + 12 * i0] * b_z[i0];
}
x_aposteriori[i] = x_apriori[i] + f0;
}
/* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 6; i1++) {
f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
}
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1
+ 12 * i0];
}
}
}
} else {
/* 'attitudeKalmanfilter:181' else */
/* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))
{
/* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */
/* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */
/* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */
/* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */
/* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */
/* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */
/* observation matrix */
/* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */
/* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */
/* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
/* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
/* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 6; i0++) {
d_P_apriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
iv7[i1 + 12 * i0];
}
}
}
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 12; i0++) {
c_K_k[i + 6 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +
12 * i0];
}
}
for (i0 = 0; i0 < 6; i0++) {
fv2[i + 6 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *
i0];
}
}
}
b_K_k[0] = r[0];
b_K_k[6] = 0.0F;
b_K_k[12] = 0.0F;
b_K_k[18] = 0.0F;
b_K_k[24] = 0.0F;
b_K_k[30] = 0.0F;
b_K_k[1] = 0.0F;
b_K_k[7] = r[0];
b_K_k[13] = 0.0F;
b_K_k[19] = 0.0F;
b_K_k[25] = 0.0F;
b_K_k[31] = 0.0F;
b_K_k[2] = 0.0F;
b_K_k[8] = 0.0F;
b_K_k[14] = r[0];
b_K_k[20] = 0.0F;
b_K_k[26] = 0.0F;
b_K_k[32] = 0.0F;
b_K_k[3] = 0.0F;
b_K_k[9] = 0.0F;
b_K_k[15] = 0.0F;
b_K_k[21] = r[2];
b_K_k[27] = 0.0F;
b_K_k[33] = 0.0F;
b_K_k[4] = 0.0F;
b_K_k[10] = 0.0F;
b_K_k[16] = 0.0F;
b_K_k[22] = 0.0F;
b_K_k[28] = r[2];
b_K_k[34] = 0.0F;
b_K_k[5] = 0.0F;
b_K_k[11] = 0.0F;
b_K_k[17] = 0.0F;
b_K_k[23] = 0.0F;
b_K_k[29] = 0.0F;
b_K_k[35] = r[2];
for (i = 0; i < 6; i++) {
for (i0 = 0; i0 < 6; i0++) {
c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
}
}
c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
/* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */
for (i = 0; i < 3; i++) {
b_z[i] = z[i];
}
for (i = 0; i < 3; i++) {
b_z[i + 3] = z[i + 6];
}
for (i = 0; i < 6; i++) {
fv3[i] = 0.0F;
for (i0 = 0; i0 < 12; i0++) {
fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];
}
c_z[i] = b_z[i] - fv3[i];
}
for (i = 0; i < 12; i++) {
f0 = 0.0F;
for (i0 = 0; i0 < 6; i0++) {
f0 += c_K_k[i + 12 * i0] * c_z[i0];
}
x_aposteriori[i] = x_apriori[i] + f0;
}
/* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
b_eye(dv1);
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
f0 = 0.0F;
for (i1 = 0; i1 < 6; i1++) {
f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];
}
b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
}
}
for (i = 0; i < 12; i++) {
for (i0 = 0; i0 < 12; i0++) {
P_aposteriori[i + 12 * i0] = 0.0F;
for (i1 = 0; i1 < 12; i1++) {
P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *
P_apriori[i1 + 12 * i0];
}
}
}
} else {
/* 'attitudeKalmanfilter:202' else */
/* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */
for (i = 0; i < 12; i++) {
x_aposteriori[i] = x_apriori[i];
}
/* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */
memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));
}
}
}
}
/* % euler anglels extraction */
/* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
for (i = 0; i < 3; i++) {
x_n_b[i] = -x_aposteriori[i + 6];
}
rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);
/* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */
rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&
x_aposteriori[9]), wak);
/* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */
for (i = 0; i < 3; i++) {
x_n_b[i] = wak[i];
}
cross(z_n_b, x_n_b, wak);
/* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */
for (i = 0; i < 3; i++) {
x_n_b[i] = wak[i];
}
rdivide(x_n_b, norm(wak), wak);
/* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */
cross(wak, z_n_b, x_n_b);
/* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */
for (i = 0; i < 3; i++) {
b_x_aposteriori_k[i] = x_n_b[i];
}
rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);
/* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
for (i = 0; i < 3; i++) {
Rot_matrix[i] = x_n_b[i];
Rot_matrix[3 + i] = wak[i];
Rot_matrix[6 + i] = z_n_b[i];
}
/* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
/* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */
/* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
/* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */
eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
}
/* Function Definitions */
void MechanicalPointForce(const emlrtStack *sp, const emxArray_real_T
*particlePosition, const emxArray_real_T *pointSourcePosition, real_T
forceDirection, real_T forceMagnitude, real_T cutoff, emxArray_real_T *force)
{
uint32_T sz[2];
int32_T ix;
emxArray_real_T *forceTemp;
int32_T loop_ub;
emxArray_real_T *forceMag;
int32_T vlen;
int32_T sIdx;
emxArray_real_T *forceDir;
emxArray_real_T *distToSource;
emxArray_int32_T *r0;
emxArray_boolean_T *r1;
emxArray_int32_T *r2;
emxArray_real_T *x;
emxArray_real_T *b_x;
emxArray_real_T *r3;
emxArray_real_T *r4;
emxArray_real_T *b_pointSourcePosition;
emxArray_real_T *b_forceDir;
emxArray_real_T *c_forceDir;
int32_T k;
int32_T vstride;
int32_T iy;
int32_T ixstart;
boolean_T overflow;
real_T s;
boolean_T b0;
uint32_T varargin_2[2];
boolean_T p;
boolean_T exitg1;
int32_T iv0[1];
int32_T iv1[2];
int32_T b_force[2];
int32_T iv2[1];
int32_T b_iy;
int32_T c_iy;
int32_T b_forceTemp[2];
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
emlrtStack e_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &c_st;
d_st.tls = c_st.tls;
e_st.prev = &d_st;
e_st.tls = d_st.tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
/* apply mechanical (push or pull) force on particles */
/* mechanicalForce is a logical flag */
/* particlPosition is a N by 3 vector of particle position */
/* pointSourcePosition is the position of force sources */
/* forceDirection is either -1 for 'in' or 1 for 'out' */
/* forceMagnitude is a positive number between 0 and 1 */
/* cutoff is the maximal direction the force operates on particle relative */
/* to the pointSourcePosition */
/* the output is a vector of N by 3 of delta position to th */
for (ix = 0; ix < 2; ix++) {
sz[ix] = (uint32_T)particlePosition->size[ix];
}
emxInit_real_T(sp, &forceTemp, 2, &c_emlrtRTEI, true);
ix = forceTemp->size[0] * forceTemp->size[1];
forceTemp->size[0] = (int32_T)sz[0];
emxEnsureCapacity(sp, (emxArray__common *)forceTemp, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
ix = forceTemp->size[0] * forceTemp->size[1];
forceTemp->size[1] = (int32_T)sz[1];
emxEnsureCapacity(sp, (emxArray__common *)forceTemp, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = (int32_T)sz[0] * (int32_T)sz[1];
for (ix = 0; ix < loop_ub; ix++) {
forceTemp->data[ix] = 0.0;
}
for (ix = 0; ix < 2; ix++) {
sz[ix] = (uint32_T)particlePosition->size[ix];
}
ix = force->size[0] * force->size[1];
force->size[0] = (int32_T)sz[0];
emxEnsureCapacity(sp, (emxArray__common *)force, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
ix = force->size[0] * force->size[1];
force->size[1] = (int32_T)sz[1];
emxEnsureCapacity(sp, (emxArray__common *)force, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
loop_ub = (int32_T)sz[0] * (int32_T)sz[1];
for (ix = 0; ix < loop_ub; ix++) {
force->data[ix] = 0.0;
}
emxInit_real_T(sp, &forceMag, 2, &d_emlrtRTEI, true);
vlen = particlePosition->size[0];
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
vlen = particlePosition->size[0];
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[1] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
loop_ub = particlePosition->size[0] * particlePosition->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceMag->data[ix] = 0.0;
}
sIdx = 0;
emxInit_real_T(sp, &forceDir, 2, &e_emlrtRTEI, true);
b_emxInit_real_T(sp, &distToSource, 1, &f_emlrtRTEI, true);
emxInit_int32_T(sp, &r0, 1, &emlrtRTEI, true);
emxInit_boolean_T(sp, &r1, 2, &emlrtRTEI, true);
emxInit_int32_T(sp, &r2, 1, &emlrtRTEI, true);
emxInit_real_T(sp, &x, 2, &emlrtRTEI, true);
b_emxInit_real_T(sp, &b_x, 1, &emlrtRTEI, true);
b_emxInit_real_T(sp, &r3, 1, &emlrtRTEI, true);
b_emxInit_real_T(sp, &r4, 1, &emlrtRTEI, true);
emxInit_real_T(sp, &b_pointSourcePosition, 2, &emlrtRTEI, true);
b_emxInit_real_T(sp, &b_forceDir, 1, &emlrtRTEI, true);
emxInit_real_T(sp, &c_forceDir, 2, &emlrtRTEI, true);
while (sIdx <= pointSourcePosition->size[0] - 1) {
loop_ub = pointSourcePosition->size[1];
ix = pointSourcePosition->size[0];
if ((sIdx + 1 >= 1) && (sIdx + 1 < ix)) {
vlen = sIdx + 1;
} else {
vlen = emlrtDynamicBoundsCheckR2012b(sIdx + 1, 1, ix, (emlrtBCInfo *)
&e_emlrtBCI, sp);
}
ix = b_pointSourcePosition->size[0] * b_pointSourcePosition->size[1];
b_pointSourcePosition->size[0] = 1;
b_pointSourcePosition->size[1] = loop_ub;
emxEnsureCapacity(sp, (emxArray__common *)b_pointSourcePosition, ix,
(int32_T)sizeof(real_T), &emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
b_pointSourcePosition->data[b_pointSourcePosition->size[0] * ix] =
pointSourcePosition->data[(vlen + pointSourcePosition->size[0] * ix) - 1];
}
st.site = &emlrtRSI;
bsxfun(&st, particlePosition, b_pointSourcePosition, forceDir);
/* Find the distance between the particles and the source */
st.site = &b_emlrtRSI;
b_st.site = &h_emlrtRSI;
c_st.site = &i_emlrtRSI;
d_st.site = &j_emlrtRSI;
for (ix = 0; ix < 2; ix++) {
sz[ix] = (uint32_T)forceDir->size[ix];
}
ix = x->size[0] * x->size[1];
x->size[0] = (int32_T)sz[0];
x->size[1] = (int32_T)sz[1];
emxEnsureCapacity(&d_st, (emxArray__common *)x, ix, (int32_T)sizeof(real_T),
&b_emlrtRTEI);
if (dimagree(x, forceDir)) {
} else {
emlrtErrorWithMessageIdR2012b(&d_st, &b_emlrtRTEI, "MATLAB:dimagree", 0);
}
ix = (int32_T)sz[0] * (int32_T)sz[1];
for (k = 0; k < ix; k++) {
x->data[k] = forceDir->data[k] * forceDir->data[k];
}
st.site = &b_emlrtRSI;
b_st.site = &k_emlrtRSI;
c_st.site = &l_emlrtRSI;
for (ix = 0; ix < 2; ix++) {
sz[ix] = (uint32_T)x->size[ix];
}
ix = b_x->size[0];
b_x->size[0] = (int32_T)sz[0];
emxEnsureCapacity(&c_st, (emxArray__common *)b_x, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
if ((x->size[0] == 0) || (x->size[1] == 0)) {
ix = b_x->size[0];
b_x->size[0] = (int32_T)sz[0];
emxEnsureCapacity(&c_st, (emxArray__common *)b_x, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = (int32_T)sz[0];
for (ix = 0; ix < loop_ub; ix++) {
b_x->data[ix] = 0.0;
}
} else {
vlen = x->size[1];
vstride = x->size[0];
iy = -1;
ixstart = -1;
d_st.site = &m_emlrtRSI;
overflow = (x->size[0] > 2147483646);
if (overflow) {
e_st.site = &g_emlrtRSI;
check_forloop_overflow_error(&e_st);
}
for (loop_ub = 1; loop_ub <= vstride; loop_ub++) {
ixstart++;
ix = ixstart;
s = x->data[ixstart];
d_st.site = &n_emlrtRSI;
if (2 > vlen) {
b0 = false;
} else {
b0 = (vlen > 2147483646);
}
if (b0) {
e_st.site = &g_emlrtRSI;
check_forloop_overflow_error(&e_st);
}
for (k = 2; k <= vlen; k++) {
ix += vstride;
s += x->data[ix];
}
iy++;
b_x->data[iy] = s;
}
}
st.site = &b_emlrtRSI;
ix = distToSource->size[0];
distToSource->size[0] = b_x->size[0];
emxEnsureCapacity(&st, (emxArray__common *)distToSource, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
distToSource->data[ix] = b_x->data[ix];
}
for (k = 0; k < b_x->size[0]; k++) {
if (b_x->data[k] < 0.0) {
b_st.site = &o_emlrtRSI;
eml_error(&b_st);
}
}
for (k = 0; k < b_x->size[0]; k++) {
distToSource->data[k] = muDoubleScalarSqrt(distToSource->data[k]);
}
/* Normalize the forceDirection */
iy = 0;
while (iy < 3) {
loop_ub = forceDir->size[0];
ix = r2->size[0];
r2->size[0] = loop_ub;
emxEnsureCapacity(sp, (emxArray__common *)r2, ix, (int32_T)sizeof(int32_T),
&emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
r2->data[ix] = ix;
}
ix = forceDir->size[1];
ixstart = 1 + iy;
emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&c_emlrtBCI,
sp);
st.site = &c_emlrtRSI;
ix = forceDir->size[1];
ixstart = 1 + iy;
emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&d_emlrtBCI,
&st);
ix = forceDir->size[0];
sz[0] = (uint32_T)ix;
sz[1] = 1U;
varargin_2[0] = (uint32_T)distToSource->size[0];
varargin_2[1] = 1U;
overflow = false;
p = true;
k = 0;
exitg1 = false;
while ((!exitg1) && (k < 2)) {
if (!((int32_T)sz[k] == (int32_T)varargin_2[k])) {
p = false;
exitg1 = true;
} else {
k++;
}
}
if (!p) {
} else {
overflow = true;
}
if (overflow) {
} else {
emlrtErrorWithMessageIdR2012b(&st, &l_emlrtRTEI, "MATLAB:dimagree", 0);
}
loop_ub = forceDir->size[0];
ix = b_x->size[0];
b_x->size[0] = loop_ub;
emxEnsureCapacity(&st, (emxArray__common *)b_x, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
b_x->data[ix] = forceDir->data[ix + forceDir->size[0] * iy] /
distToSource->data[ix];
}
iv0[0] = r2->size[0];
emlrtSubAssignSizeCheckR2012b(iv0, 1, *(int32_T (*)[1])b_x->size, 1,
(emlrtECInfo *)&d_emlrtECI, sp);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceDir->data[r2->data[ix] + forceDir->size[0] * iy] = b_x->data[ix];
}
/* bsxfun(@rdivide,forceDir,distToSource); */
iy++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
/* Multiply the */
if (forceDirection == -1.0) {
ix = r4->size[0];
r4->size[0] = distToSource->size[0];
emxEnsureCapacity(sp, (emxArray__common *)r4, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
loop_ub = distToSource->size[0];
for (ix = 0; ix < loop_ub; ix++) {
r4->data[ix] = 1.0 + distToSource->data[ix];
}
rdivide(sp, forceMagnitude, r4, b_x);
vlen = b_x->size[0];
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceMag->data[ix] = 1.0 - b_x->data[ix];
}
} else {
if (forceDirection == 1.0) {
ix = r3->size[0];
r3->size[0] = distToSource->size[0];
emxEnsureCapacity(sp, (emxArray__common *)r3, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
loop_ub = distToSource->size[0];
for (ix = 0; ix < loop_ub; ix++) {
r3->data[ix] = 1.0 + distToSource->data[ix];
}
rdivide(sp, forceMagnitude, r3, b_x);
vlen = b_x->size[0];
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
ix = forceMag->size[0] * forceMag->size[1];
forceMag->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)forceMag, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceMag->data[ix] = b_x->data[ix];
}
}
}
iy = 0;
while (iy < 3) {
ix = forceDir->size[1];
ixstart = 1 + iy;
emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&b_emlrtBCI,
sp);
ix = forceDir->size[0];
iv1[0] = ix;
iv1[1] = 1;
for (ix = 0; ix < 2; ix++) {
b_force[ix] = forceMag->size[ix];
}
if ((iv1[0] != b_force[0]) || (1 != b_force[1])) {
emlrtSizeEqCheckNDR2012b(iv1, b_force, (emlrtECInfo *)&c_emlrtECI, sp);
}
loop_ub = forceTemp->size[0];
ix = r2->size[0];
r2->size[0] = loop_ub;
emxEnsureCapacity(sp, (emxArray__common *)r2, ix, (int32_T)sizeof(int32_T),
&emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
r2->data[ix] = ix;
}
ix = forceTemp->size[1];
ixstart = 1 + iy;
emlrtDynamicBoundsCheckR2012b(ixstart, 1, ix, (emlrtBCInfo *)&emlrtBCI, sp);
loop_ub = forceDir->size[0];
vlen = forceDir->size[0];
vstride = forceDir->size[0];
ix = b_forceDir->size[0];
b_forceDir->size[0] = vstride;
emxEnsureCapacity(sp, (emxArray__common *)b_forceDir, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
for (ix = 0; ix < vstride; ix++) {
b_forceDir->data[ix] = forceDir->data[ix + forceDir->size[0] * iy];
}
ix = c_forceDir->size[0] * c_forceDir->size[1];
c_forceDir->size[0] = loop_ub;
c_forceDir->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)c_forceDir, ix, (int32_T)sizeof
(real_T), &emlrtRTEI);
for (ix = 0; ix < loop_ub; ix++) {
c_forceDir->data[ix] = b_forceDir->data[ix];
}
ix = b_x->size[0];
b_x->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)b_x, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
for (ix = 0; ix < vlen; ix++) {
b_x->data[ix] = c_forceDir->data[ix] * forceMag->data[ix];
}
iv2[0] = r2->size[0];
emlrtSubAssignSizeCheckR2012b(iv2, 1, *(int32_T (*)[1])b_x->size, 1,
(emlrtECInfo *)&b_emlrtECI, sp);
loop_ub = b_x->size[0];
for (ix = 0; ix < loop_ub; ix++) {
forceTemp->data[r2->data[ix] + forceTemp->size[0] * iy] = b_x->data[ix];
}
/* bsxfun(@times,forceDir,forceTemp); */
iy++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
iy = distToSource->size[0] - 1;
vlen = 0;
for (vstride = 0; vstride <= iy; vstride++) {
if (distToSource->data[vstride] > cutoff) {
vlen++;
}
}
ix = r2->size[0];
r2->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)r2, ix, (int32_T)sizeof(int32_T),
&emlrtRTEI);
vlen = 0;
for (vstride = 0; vstride <= iy; vstride++) {
if (distToSource->data[vstride] > cutoff) {
r2->data[vlen] = vstride + 1;
vlen++;
}
}
loop_ub = forceTemp->size[1];
vstride = forceTemp->size[0];
vlen = r2->size[0];
for (ix = 0; ix < loop_ub; ix++) {
for (ixstart = 0; ixstart < vlen; ixstart++) {
iy = r2->data[ixstart];
if ((iy >= 1) && (iy < vstride)) {
b_iy = iy;
} else {
b_iy = emlrtDynamicBoundsCheckR2012b(iy, 1, vstride, (emlrtBCInfo *)
&f_emlrtBCI, sp);
}
forceTemp->data[(b_iy + forceTemp->size[0] * ix) - 1] = 0.0;
}
}
ix = r1->size[0] * r1->size[1];
r1->size[0] = forceTemp->size[0];
r1->size[1] = forceTemp->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r1, ix, (int32_T)sizeof(boolean_T),
&emlrtRTEI);
loop_ub = forceTemp->size[0] * forceTemp->size[1];
for (ix = 0; ix < loop_ub; ix++) {
r1->data[ix] = muDoubleScalarIsNaN(forceTemp->data[ix]);
}
iy = r1->size[0] * r1->size[1] - 1;
vlen = 0;
for (vstride = 0; vstride <= iy; vstride++) {
if (r1->data[vstride]) {
vlen++;
}
}
ix = r0->size[0];
r0->size[0] = vlen;
emxEnsureCapacity(sp, (emxArray__common *)r0, ix, (int32_T)sizeof(int32_T),
&emlrtRTEI);
vlen = 0;
for (vstride = 0; vstride <= iy; vstride++) {
if (r1->data[vstride]) {
r0->data[vlen] = vstride + 1;
vlen++;
}
}
vstride = forceTemp->size[0];
vlen = forceTemp->size[1];
loop_ub = r0->size[0];
for (ix = 0; ix < loop_ub; ix++) {
ixstart = vstride * vlen;
iy = r0->data[ix];
if ((iy >= 1) && (iy < ixstart)) {
c_iy = iy;
} else {
c_iy = emlrtDynamicBoundsCheckR2012b(iy, 1, ixstart, (emlrtBCInfo *)
&g_emlrtBCI, sp);
}
forceTemp->data[c_iy - 1] = 0.0;
}
for (ix = 0; ix < 2; ix++) {
b_force[ix] = force->size[ix];
}
for (ix = 0; ix < 2; ix++) {
b_forceTemp[ix] = forceTemp->size[ix];
}
if ((b_force[0] != b_forceTemp[0]) || (b_force[1] != b_forceTemp[1])) {
emlrtSizeEqCheckNDR2012b(b_force, b_forceTemp, (emlrtECInfo *)&emlrtECI,
sp);
}
ix = force->size[0] * force->size[1];
emxEnsureCapacity(sp, (emxArray__common *)force, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
vlen = force->size[0];
vstride = force->size[1];
loop_ub = vlen * vstride;
for (ix = 0; ix < loop_ub; ix++) {
force->data[ix] += forceTemp->data[ix];
}
sIdx++;
if (*emlrtBreakCheckR2012bFlagVar != 0) {
emlrtBreakCheckR2012b(sp);
}
}
emxFree_real_T(&c_forceDir);
emxFree_real_T(&b_forceDir);
emxFree_real_T(&b_pointSourcePosition);
emxFree_real_T(&r4);
emxFree_real_T(&r3);
emxFree_real_T(&b_x);
emxFree_real_T(&x);
emxFree_int32_T(&r2);
emxFree_boolean_T(&r1);
emxFree_int32_T(&r0);
emxFree_real_T(&distToSource);
emxFree_real_T(&forceDir);
emxFree_real_T(&forceMag);
emxFree_real_T(&forceTemp);
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
void drr_151213_XRayInParam(const emlrtStack *sp, const emxArray_boolean_T
*voxel_data, const real_T voxel_size_mm[3], const real_T detector_dimensions[2],
const real_T XRayIntrinsicParam[12], const real_T voxel_corner_min[3], const
real_T T_carm[16], emxArray_real_T *projection)
{
const mxArray *y;
static const int32_T iv0[2] = { 1, 32 };
const mxArray *m0;
char_T cv0[32];
int32_T i;
static const char_T cv1[32] = { 'v', 'o', 'x', 'e', 'l', '_', 'd', 'a', 't',
'a', ' ', 'm', 'u', 's', 't', ' ', 'b', 'e', ' ', '3', '-', 'd', 'i', 'm',
'e', 'n', 's', 'i', 'o', 'n', 'a', 'l' };
int32_T i0;
real_T pixel_size_mm_h;
real_T pixel_size_mm_w;
uint32_T voxDim[3];
real_T voxPlanes__max[3];
real_T source[3];
real_T pixel_size_mmel_wn[3];
real_T pixel_size_mmel_hn[3];
real_T corner[3];
int32_T ih;
int32_T iw;
real_T tstep[3];
real_T tnext[3];
real_T pixel_point_mm[3];
real_T ray_source2pixel[3];
real_T b_voxPlanes__max[3];
real_T b_ray_source2pixel[3];
real_T t_plane_min[3];
real_T t_plane_max[3];
real_T tmax;
int32_T pixel_size_mmIntensity;
boolean_T exitg8;
boolean_T exitg7;
boolean_T exitg6;
real_T t_larger[4];
real_T temp;
boolean_T exitg5;
boolean_T exitg4;
boolean_T exitg3;
real_T t_smaller[4];
int32_T itmp;
boolean_T exitg2;
boolean_T exitg1;
real_T iz;
real_T tx;
real_T ty;
real_T tz;
int32_T i1;
int32_T i2;
int32_T i3;
emlrtStack st;
emlrtStack b_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
/* % Modificiation Notes */
/* 15.12.04 */
/* - Release 기존의 파일을 참고로 하여 고성영이 수정하였음. */
/* - DRR을 완벽하게 하지 않고, voxel에 한점이라도 만나면 on으로 계산함 */
/* - 계산속도가 향상되었음 */
/* - 젬스에 있는 X-ray와 테스트하여 검증하였음 */
/* 15 12 13 : The function input has been changed to utilize the x-ray */
/* intrinsic parameter provided by GEMSS % 151213 kosy */
/* %function [projection] = drr (voxel_data, voxel_size_mm, detector_dimensions, pixel_size_mm, isocenter_mm, cbct_angles_deg) */
/* Creates a 2D projection at each cbct_angle of voxel_data. the projection */
/* axis is defined by the isocenter to which the source and center of */
/* the detector are aligned. This simulation assumes standard Cone Beam CT */
/* geometry (source to isocenter distance is 100 cm and source to detector */
/* distance is 150cm). */
/* */
/* voxel_data: must be a 3 dimensional matrix (typically of CT data) */
/* voxel_size_mm: a 3 element vector listing the size (in mm) of the voxels */
/* along each dimension */
/* detector_dimension: a 2 element vector listing the dimensions (number of */
/* pixels) in each dimension (u,v) */
/* pixel_size_mm: a number defining the height and width of each pixel */
/* (assumes square pixel) */
/* isocenter_mm: a 3 element vector pointing from the origin (corner) of the */
/* matrix element(1,1,1) to the isocenter */
/* cbct_angles_deg: a list of angles to generate projections */
/* */
/* Retrun Variable */
/* projection: a 3D matrix with the 3rd dimension representing the angle of */
/* roatation */
/* */
/* { */
/* Author: Michael M. Folkerts [email protected] */
/* Institution: UCSD Physics, UTSW Radiation Oncology */
/* Updated: 2014-July. */
/* Notes: Siddon's Incremental algorithm | modified to read in 3D matlab matrix | Simplified Input | No guarantees!! */
/* */
/* References: */
/* R.L. Siddon, */
/* "Fast calculation of the exact radiological path for a three-dimensional CT */
/* array," Medical Physics 12, 252-255 (1985). */
/* */
/* F. Jacobs, E. Sundermann, B. De Sutter, M. Christiaens and I. Lemahieu, */
/* "A fast algorithm to calculate the exact radiological path through a pixel_size_mmel or voxel space," */
/* Journal of Computing and Information Technology 6, 89-94 (1998). */
/* */
/* G. Han, Z. Liang and J. You, */
/* "A fast ray tracing technique for TCT and ECT studies," */
/* IEEE Medical Imaging Conference 1999. */
/* } */
/* function [projection] = drr_good_151204 (voxel_data, voxel_size_mm, detector_dimensions, pixel_size_mm, voxel_corner_min, T_carm) */
/* if(0) */
/* voxel_data = OUTPUTgrid; */
/* voxel_size_mm = voxel_size; */
/* detector_dimensions = detector_dimension; */
/* pixel_size_mm = pixel_size; */
/* isocenter_mm = isocenter; */
/* T_carm: Transformation matrix of C-arm (that is set at the middle of */
/* detector & source) with respect to Voxel coordinates */
/* tic; */
/* this will verify the size: */
if (eml_ndims_varsized(*(int32_T (*)[3])voxel_data->size) != 3) {
st.site = &emlrtRSI;
y = NULL;
m0 = emlrtCreateCharArray(2, iv0);
for (i = 0; i < 32; i++) {
cv0[i] = cv1[i];
}
emlrtInitCharArrayR2013a(&st, 32, m0, cv0);
emlrtAssign(&y, m0);
b_st.site = &b_emlrtRSI;
error(&b_st, y, &emlrtMCI);
}
/* constants: */
/* .0001; */
/* .0001; */
/* sounce to imager(detector) distance */
/* source to axis distance %% RRI set the coordinate at the middle between source and the detector */
/* initialize memory for speed: */
i0 = projection->size[0] * projection->size[1];
pixel_size_mm_h = emlrtNonNegativeCheckFastR2012b(detector_dimensions[0],
&b_emlrtDCI, sp);
projection->size[0] = (int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_h,
&emlrtDCI, sp);
pixel_size_mm_h = emlrtNonNegativeCheckFastR2012b(detector_dimensions[1],
&d_emlrtDCI, sp);
projection->size[1] = (int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_h,
&c_emlrtDCI, sp);
emxEnsureCapacity(sp, (emxArray__common *)projection, i0, (int32_T)sizeof
(real_T), &emlrtRTEI);
pixel_size_mm_h = emlrtNonNegativeCheckFastR2012b(detector_dimensions[0],
&b_emlrtDCI, sp);
pixel_size_mm_w = emlrtNonNegativeCheckFastR2012b(detector_dimensions[1],
&d_emlrtDCI, sp);
i = (int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_h, &emlrtDCI, sp) *
(int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_w, &c_emlrtDCI, sp);
for (i0 = 0; i0 < i; i0++) {
projection->data[i0] = 0.0;
}
for (i0 = 0; i0 < 3; i0++) {
voxDim[i0] = (uint32_T)voxel_data->size[i0];
}
/* [size(voxel_data,1), size(voxel_data,2), size(voxel_data,3)]; */
/* voxDim_x = voxDim(1); */
/* voxDim_y = voxDim(2); */
/* voxDim_z = voxDim(3); */
/* difine voxel boundaries: */
/* vector from origin to source */
/* vector from origin to CENTER of detector */
/* extract the key information from the intrinsic parameters % 151213 kosy modi */
pixel_size_mm_h = 1105.0 / XRayIntrinsicParam[0];
pixel_size_mm_w = 1105.0 / XRayIntrinsicParam[4];
/* vector pointing left, parallel to detector */
/* define incremental vectors: */
/* define upper lefthand corner of detector: */
/* corner = detector + ( center_pixel_w*pixel_size_mmel.wn + center_pixel_h*pixel_size_mmel.hn ); */
for (i = 0; i < 3; i++) {
voxPlanes__max[i] = voxel_corner_min[i] + voxel_size_mm[i] * (real_T)
voxDim[i];
/* define up: */
/* up = [0,0,1]; */
/* width of projection image */
/* height of projection image */
/* direction from the detector to the source */
/* end initialization timer: */
/* init_time = toc */
/* start tracing timer: */
/* tic; */
source[i] = 552.5 * T_carm[4 + i] + T_carm[12 + i];
/* define pixel_size_mmel vectors: */
/* length of pixel_size_mmel */
pixel_size_mmel_wn[i] = -pixel_size_mm_w * T_carm[i];
pixel_size_mmel_hn[i] = -pixel_size_mm_h * T_carm[8 + i];
corner[i] = (-552.5 * T_carm[4 + i] + T_carm[12 + i]) +
(detector_dimensions[0] * (pixel_size_mm_w * T_carm[i]) +
detector_dimensions[1] * (pixel_size_mm_h * T_carm[8 + i])) / 2.0;
}
emlrtForLoopVectorCheckR2012b(1.0, 1.0, detector_dimensions[0], mxDOUBLE_CLASS,
(int32_T)detector_dimensions[0], &d_emlrtRTEI,
sp);
ih = 1;
while (ih - 1 <= (int32_T)detector_dimensions[0] - 1) {
/* if(mod(ih,100)==0),fprintf('height:\t%i...\n',ih);end */
emlrtForLoopVectorCheckR2012b(1.0, 1.0, detector_dimensions[1],
mxDOUBLE_CLASS, (int32_T)detector_dimensions[1], &c_emlrtRTEI, sp);
iw = 1;
while (iw - 1 <= (int32_T)detector_dimensions[1] - 1) {
/* pixel_point_mm = corner + (ih-1)*pixel_size_mmel.hp + (iw-1)*pixel_size_mmel.wp; %ray end point */
/* ray to be traced */
/* find parametrized (t) voxel plane (min or max) intersections: */
/* PLANE = P1 + t(P2-P1) */
/* SK added */
/* t_x = (i-x1) / (x2-x1), t_y = (j-y1) / (y2-y1), t_z = (k-z1) / (z2-z1) */
for (i = 0; i < 3; i++) {
pixel_size_mm_h = (corner[i] + ((1.0 + (real_T)(ih - 1)) - 1.0) *
pixel_size_mmel_hn[i]) + ((1.0 + (real_T)(iw - 1)) -
1.0) * pixel_size_mmel_wn[i];
/* ray end point */
pixel_size_mm_w = pixel_size_mm_h - source[i];
tstep[i] = voxel_corner_min[i] - source[i];
tnext[i] = pixel_size_mm_w + 2.2250738585072014E-308;
b_voxPlanes__max[i] = voxPlanes__max[i] - source[i];
b_ray_source2pixel[i] = pixel_size_mm_w + 2.2250738585072014E-308;
pixel_point_mm[i] = pixel_size_mm_h;
ray_source2pixel[i] = pixel_size_mm_w;
}
rdivide(tstep, tnext, t_plane_min);
rdivide(b_voxPlanes__max, b_ray_source2pixel, t_plane_max);
/* compute (parametric) intersection values */
/* tmin = max { min{tx(0), tx(Nx)}, min{ty(0), ty(Ny)], min{tz(0), tz(Nz)}, 0.0 } */
/* tmax = min { max{tx(0), tx(Nx)}, max{ty(0), ty(Ny)], max{tz(0), tz(Nz)}, 1.0 } */
/* t_larger = [ max([t_plane_min(1), t_plane_max(1)]), max([t_plane_min(2), t_plane_max(2)]), max([t_plane_min(3), t_plane_max(3)]), 1.0 ]; */
/* t_smaller = [ min([t_plane_min(1), t_plane_max(1)]), min([t_plane_min(2), t_plane_max(2)]), min([t_plane_min(3), t_plane_max(3)]), 0.0 ]; */
i = 1;
tmax = t_plane_min[0];
if (muDoubleScalarIsNaN(t_plane_min[0])) {
pixel_size_mmIntensity = 2;
exitg8 = false;
while ((!exitg8) && (pixel_size_mmIntensity < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[0])) {
tmax = t_plane_max[0];
exitg8 = true;
} else {
pixel_size_mmIntensity = 3;
}
}
}
if ((i < 2) && (t_plane_max[0] > tmax)) {
tmax = t_plane_max[0];
}
i = 1;
pixel_size_mm_h = t_plane_min[1];
if (muDoubleScalarIsNaN(t_plane_min[1])) {
pixel_size_mmIntensity = 2;
exitg7 = false;
while ((!exitg7) && (pixel_size_mmIntensity < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[1])) {
pixel_size_mm_h = t_plane_max[1];
exitg7 = true;
} else {
pixel_size_mmIntensity = 3;
}
}
}
if ((i < 2) && (t_plane_max[1] > pixel_size_mm_h)) {
pixel_size_mm_h = t_plane_max[1];
}
i = 1;
pixel_size_mm_w = t_plane_min[2];
if (muDoubleScalarIsNaN(t_plane_min[2])) {
pixel_size_mmIntensity = 2;
exitg6 = false;
while ((!exitg6) && (pixel_size_mmIntensity < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[2])) {
pixel_size_mm_w = t_plane_max[2];
exitg6 = true;
} else {
pixel_size_mmIntensity = 3;
}
}
}
if ((i < 2) && (t_plane_max[2] > pixel_size_mm_w)) {
pixel_size_mm_w = t_plane_max[2];
}
t_larger[0] = tmax;
t_larger[1] = pixel_size_mm_h;
t_larger[2] = pixel_size_mm_w;
t_larger[3] = 1.0;
i = 1;
temp = t_plane_min[0];
if (muDoubleScalarIsNaN(t_plane_min[0])) {
pixel_size_mmIntensity = 2;
exitg5 = false;
while ((!exitg5) && (pixel_size_mmIntensity < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[0])) {
temp = t_plane_max[0];
exitg5 = true;
} else {
pixel_size_mmIntensity = 3;
}
}
}
if ((i < 2) && (t_plane_max[0] < temp)) {
temp = t_plane_max[0];
}
i = 1;
pixel_size_mm_h = t_plane_min[1];
if (muDoubleScalarIsNaN(t_plane_min[1])) {
pixel_size_mmIntensity = 2;
exitg4 = false;
while ((!exitg4) && (pixel_size_mmIntensity < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[1])) {
pixel_size_mm_h = t_plane_max[1];
exitg4 = true;
} else {
pixel_size_mmIntensity = 3;
}
}
}
if ((i < 2) && (t_plane_max[1] < pixel_size_mm_h)) {
pixel_size_mm_h = t_plane_max[1];
}
i = 1;
pixel_size_mm_w = t_plane_min[2];
if (muDoubleScalarIsNaN(t_plane_min[2])) {
pixel_size_mmIntensity = 2;
exitg3 = false;
while ((!exitg3) && (pixel_size_mmIntensity < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[2])) {
pixel_size_mm_w = t_plane_max[2];
exitg3 = true;
} else {
pixel_size_mmIntensity = 3;
}
}
}
if ((i < 2) && (t_plane_max[2] < pixel_size_mm_w)) {
pixel_size_mm_w = t_plane_max[2];
}
t_smaller[0] = temp;
t_smaller[1] = pixel_size_mm_h;
t_smaller[2] = pixel_size_mm_w;
t_smaller[3] = 0.0;
i = 1;
itmp = 0;
if (muDoubleScalarIsNaN(temp)) {
pixel_size_mmIntensity = 1;
exitg2 = false;
while ((!exitg2) && (pixel_size_mmIntensity + 1 < 5)) {
i = pixel_size_mmIntensity + 1;
if (!muDoubleScalarIsNaN(t_smaller[pixel_size_mmIntensity])) {
temp = t_smaller[pixel_size_mmIntensity];
itmp = pixel_size_mmIntensity;
exitg2 = true;
} else {
pixel_size_mmIntensity++;
}
}
}
if (i < 4) {
while (i + 1 < 5) {
if (t_smaller[i] > temp) {
temp = t_smaller[i];
itmp = i;
}
i++;
}
}
i = 1;
if (muDoubleScalarIsNaN(tmax)) {
pixel_size_mmIntensity = 2;
exitg1 = false;
while ((!exitg1) && (pixel_size_mmIntensity < 5)) {
i = pixel_size_mmIntensity;
if (!muDoubleScalarIsNaN(t_larger[pixel_size_mmIntensity - 1])) {
tmax = t_larger[pixel_size_mmIntensity - 1];
exitg1 = true;
} else {
pixel_size_mmIntensity++;
}
}
}
if (i < 4) {
while (i + 1 < 5) {
if (t_larger[i] < tmax) {
tmax = t_larger[i];
}
i++;
}
}
for (i0 = 0; i0 < 3; i0++) {
pixel_point_mm[i0] = (real_T)(pixel_point_mm[i0] < source[i0]) * -2.0 +
1.0;
}
if (temp < tmax) {
/* if ray intersects volume */
/* find index for each dimension: */
for (i = 0; i < 3; i++) {
pixel_size_mm_h = muDoubleScalarFloor((((ray_source2pixel[i] * temp +
source[i]) - voxel_corner_min[i]) + 2.2250738585072014E-308) /
voxel_size_mm[i]);
/* (parametric) intersection values... */
/* makes 0 or 1 */
tnext[i] = (voxel_corner_min[i] + ((pixel_size_mm_h +
(pixel_point_mm[i] + 1.0) / 2.0) * voxel_size_mm[i] - source[i])) /
(ray_source2pixel[i] + 2.2250738585072014E-308);
/* parametric value for next plane intersection */
tstep[i] = muDoubleScalarAbs(voxel_size_mm[i] / (ray_source2pixel[i] +
2.2250738585072014E-308));
t_plane_min[i] = pixel_size_mm_h;
}
/* parametric step size */
/* address special cases... */
if (temp == t_plane_max[emlrtDynamicBoundsCheckFastR2012b(itmp + 1, 1, 3,
&c_emlrtBCI, sp) - 1]) {
/* if intersection is a "max" plane */
t_plane_min[itmp] = (real_T)voxDim[itmp] - 1.0;
tnext[itmp] = temp + tstep[itmp];
/* next plane crossing */
} else {
t_plane_min[itmp] = 0.0;
tnext[itmp] = temp + tstep[itmp];
/* next plane crossing */
}
/* voxel index values(add one for matlab): */
pixel_size_mm_h = t_plane_min[0] + 1.0;
pixel_size_mm_w = t_plane_min[1] + 1.0;
iz = t_plane_min[2] + 1.0;
tx = tnext[0];
ty = tnext[1];
tz = tnext[2];
pixel_size_mmIntensity = 0;
/* uncomment to generate P-matrix: */
/* pixel_size_mmNum = 1; */
/* len = norm(ray_source2pixel); % ray length */
while ((temp + 0.0001 < tmax) && (!(pixel_size_mm_h * pixel_size_mm_w *
iz == 0.0))) {
if ((tx < ty) && (tx < tz)) {
/* 실제 DRR을 그리지 않고, 한점이라도 지나면 해당 점에 포함시킴 */
i0 = voxel_data->size[0];
i = (int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_h,
&k_emlrtDCI, sp);
itmp = voxel_data->size[1];
i1 = (int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_w,
&l_emlrtDCI, sp);
i2 = voxel_data->size[2];
i3 = (int32_T)emlrtIntegerCheckFastR2012b(iz, &m_emlrtDCI, sp);
if (voxel_data->data[((emlrtDynamicBoundsCheckFastR2012b(i, 1, i0,
&j_emlrtBCI, sp) + voxel_data->size[0] *
(emlrtDynamicBoundsCheckFastR2012b(i1, 1,
itmp, &k_emlrtBCI, sp) - 1)) + voxel_data->size[0] *
voxel_data->size[1] *
(emlrtDynamicBoundsCheckFastR2012b(i3, 1, i2,
&l_emlrtBCI, sp) - 1)) - 1]) {
pixel_size_mmIntensity = 255;
tmax = rtMinusInf;
}
temp = tx;
tx += tstep[0];
pixel_size_mm_h += pixel_point_mm[0];
} else if (ty < tz) {
/* 실제 DRR을 그리지 않고, 한점이라도 지나면 해당 점에 포함시킴 */
i0 = voxel_data->size[0];
i = (int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_h,
&h_emlrtDCI, sp);
itmp = voxel_data->size[1];
i1 = (int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_w,
&i_emlrtDCI, sp);
i2 = voxel_data->size[2];
i3 = (int32_T)emlrtIntegerCheckFastR2012b(iz, &j_emlrtDCI, sp);
if (voxel_data->data[((emlrtDynamicBoundsCheckFastR2012b(i, 1, i0,
&g_emlrtBCI, sp) + voxel_data->size[0] *
(emlrtDynamicBoundsCheckFastR2012b(i1, 1,
itmp, &h_emlrtBCI, sp) - 1)) + voxel_data->size[0] *
voxel_data->size[1] *
(emlrtDynamicBoundsCheckFastR2012b(i3, 1, i2,
&i_emlrtBCI, sp) - 1)) - 1]) {
pixel_size_mmIntensity = 255;
tmax = rtMinusInf;
}
temp = ty;
ty += tstep[1];
pixel_size_mm_w += pixel_point_mm[1];
} else {
/* 실제 DRR을 그리지 않고, 한점이라도 지나면 해당 점에 포함시킴 */
i0 = voxel_data->size[0];
i = (int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_h,
&e_emlrtDCI, sp);
itmp = voxel_data->size[1];
i1 = (int32_T)emlrtIntegerCheckFastR2012b(pixel_size_mm_w,
&f_emlrtDCI, sp);
i2 = voxel_data->size[2];
i3 = (int32_T)emlrtIntegerCheckFastR2012b(iz, &g_emlrtDCI, sp);
if (voxel_data->data[((emlrtDynamicBoundsCheckFastR2012b(i, 1, i0,
&d_emlrtBCI, sp) + voxel_data->size[0] *
(emlrtDynamicBoundsCheckFastR2012b(i1, 1,
itmp, &e_emlrtBCI, sp) - 1)) + voxel_data->size[0] *
voxel_data->size[1] *
(emlrtDynamicBoundsCheckFastR2012b(i3, 1, i2,
&f_emlrtBCI, sp) - 1)) - 1]) {
pixel_size_mmIntensity = 255;
tmax = rtMinusInf;
}
temp = tz;
tz += tstep[2];
iz += pixel_point_mm[2];
}
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, sp);
}
/* end while */
i0 = projection->size[0];
i = projection->size[1];
projection->data[(emlrtDynamicBoundsCheckFastR2012b(ih, 1, i0,
&m_emlrtBCI, sp) + projection->size[0] *
(emlrtDynamicBoundsCheckFastR2012b(iw, 1, i,
&n_emlrtBCI, sp) - 1)) - 1] = pixel_size_mmIntensity;
} else {
/* if no intersections */
i0 = projection->size[0];
i = projection->size[1];
projection->data[(emlrtDynamicBoundsCheckFastR2012b(ih, 1, i0, &emlrtBCI,
sp) + projection->size[0] * (emlrtDynamicBoundsCheckFastR2012b(iw, 1,
i, &b_emlrtBCI, sp) - 1)) - 1] = 0.0;
}
/* if intersections */
/* uncomment to generate P-matrix: */
/* rayCount = rayCount + 1; */
iw++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, sp);
}
/* width */
/* fprintf('\n'); */
ih++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, sp);
}
/* height */
/* uncomment to generate P-matrix: */
/* matrix = matrix(1:mtxCount-1,:); */
/* stop trace timer: */
/* trace_time = toc */
/* fprintf('\n') */
/* function */
/* } */
}
void sammon(const emxArray_real_T *x, emxArray_real_T *y)
{
emxArray_real_T *D;
emxArray_real_T *delta;
int32_T i0;
int32_T b_D;
int32_T br;
emxArray_real_T *Dinv;
emxArray_real_T *d;
emxArray_real_T *dinv;
emxArray_real_T *c_D;
emxArray_real_T *b_delta;
emxArray_real_T *b_x;
real_T E;
int32_T i;
emxArray_real_T *deltaone;
emxArray_real_T *g;
emxArray_real_T *y2;
emxArray_real_T *H;
emxArray_real_T *s;
emxArray_real_T *C;
emxArray_real_T *b_C;
emxArray_real_T *c_delta;
emxArray_real_T *d_D;
emxArray_real_T *b_deltaone;
boolean_T exitg1;
boolean_T guard2 = FALSE;
uint32_T unnamed_idx_0;
int32_T ic;
int32_T ar;
int32_T ib;
int32_T ia;
int32_T i1;
boolean_T guard1 = FALSE;
int32_T exitg3;
boolean_T exitg2;
int32_T b_y[2];
real_T E_new;
real_T u1;
emxInit_real_T(&D, 2);
emxInit_real_T(&delta, 2);
/* #codgen */
/* */
/* SAMMON - apply Sammon's nonlinear mapping */
/* */
/* Y = SAMMON(X) applies Sammon's nonlinear mapping procedure on */
/* multivariate data X, where each row represents a pattern and each column */
/* represents a feature. On completion, Y contains the corresponding */
/* co-ordinates of each point on the map. By default, a two-dimensional */
/* map is created. Note if X contains any duplicated rows, SAMMON will */
/* fail (ungracefully). */
/* */
/* [Y,E] = SAMMON(X) also returns the value of the cost function in E (i.e. */
/* the stress of the mapping). */
/* */
/* An N-dimensional output map is generated by Y = SAMMON(X,N) . */
/* */
/* A set of optimisation options can also be specified using a third */
/* argument, Y = SAMMON(X,N,OPTS) , where OPTS is a structure with fields: */
/* */
/* MaxIter - maximum number of iterations */
/* TolFun - relative tolerance on objective function */
/* MaxHalves - maximum number of step halvings */
/* Input - {'raw','distance'} if set to 'distance', X is */
/* interpreted as a matrix of pairwise distances. */
/* Display - {'off', 'on', 'iter'} */
/* Initialisation - {'pca', 'random'} */
/* */
/* The default options structure can be retrieved by calling SAMMON with */
/* no parameters. */
/* */
/* References : */
/* */
/* [1] Sammon, John W. Jr., "A Nonlinear Mapping for Data Structure */
/* Analysis", IEEE Transactions on Computers, vol. C-18, no. 5, */
/* pp 401-409, May 1969. */
/* */
/* See also : SAMMON_TEST */
/* */
/* File : sammon.m */
/* */
/* Date : Monday 12th November 2007. */
/* */
/* Author : Gavin C. Cawley and Nicola L. C. Talbot */
/* */
/* Description : Simple vectorised MATLAB implementation of Sammon's non-linear */
/* mapping algorithm [1]. */
/* */
/* References : [1] Sammon, John W. Jr., "A Nonlinear Mapping for Data */
/* Structure Analysis", IEEE Transactions on Computers, */
/* vol. C-18, no. 5, pp 401-409, May 1969. */
/* */
/* History : 10/08/2004 - v1.00 */
/* 11/08/2004 - v1.10 Hessian made positive semidefinite */
/* 13/08/2004 - v1.11 minor optimisation */
/* 12/11/2007 - v1.20 initialisation using the first n principal */
/* components. */
/* */
/* Thanks : Dr Nick Hamilton ([email protected]) for supplying the */
/* code for implementing initialisation using the first n */
/* principal components (introduced in v1.20). */
/* */
/* To do : The current version does not take advantage of the symmetry */
/* of the distance matrix in order to allow for easy */
/* vectorisation. This may not be a good choice for very large */
/* datasets, so perhaps one day I'll get around to doing a MEX */
/* version using the BLAS library etc. for very large datasets. */
/* */
/* Copyright : (c) Dr Gavin C. Cawley, November 2007. */
/* */
/* This program is free software; you can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published by */
/* the Free Software Foundation; either version 2 of the License, or */
/* (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */
/* GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public License */
/* along with this program; if not, write to the Free Software */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */
/* */
/* use the default options structure */
/* create distance matrix unless given by parameters */
euclid(x, x, D);
/* remaining initialisation */
eye(x->size[0], delta);
i0 = D->size[0] * D->size[1];
emxEnsureCapacity((emxArray__common *)D, i0, (int32_T)sizeof(real_T));
b_D = D->size[0];
br = D->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
D->data[i0] += delta->data[i0];
}
emxInit_real_T(&Dinv, 2);
emxInit_real_T(&d, 2);
rdivide(D, Dinv);
randn(x->size[0], y);
b_euclid(y, y, d);
eye(x->size[0], delta);
i0 = d->size[0] * d->size[1];
emxEnsureCapacity((emxArray__common *)d, i0, (int32_T)sizeof(real_T));
b_D = d->size[0];
br = d->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
d->data[i0] += delta->data[i0];
}
emxInit_real_T(&dinv, 2);
emxInit_real_T(&c_D, 2);
rdivide(d, dinv);
i0 = c_D->size[0] * c_D->size[1];
c_D->size[0] = D->size[0];
c_D->size[1] = D->size[1];
emxEnsureCapacity((emxArray__common *)c_D, i0, (int32_T)sizeof(real_T));
br = D->size[0] * D->size[1];
for (i0 = 0; i0 < br; i0++) {
c_D->data[i0] = D->data[i0] - d->data[i0];
}
emxInit_real_T(&b_delta, 2);
power(c_D, delta);
i0 = b_delta->size[0] * b_delta->size[1];
b_delta->size[0] = delta->size[0];
b_delta->size[1] = delta->size[1];
emxEnsureCapacity((emxArray__common *)b_delta, i0, (int32_T)sizeof(real_T));
br = delta->size[0] * delta->size[1];
emxFree_real_T(&c_D);
for (i0 = 0; i0 < br; i0++) {
b_delta->data[i0] = delta->data[i0] * Dinv->data[i0];
}
emxInit_real_T(&b_x, 2);
c_sum(b_delta, b_x);
E = d_sum(b_x);
/* get on with it */
i = 0;
emxFree_real_T(&b_delta);
emxInit_real_T(&deltaone, 2);
emxInit_real_T(&g, 2);
emxInit_real_T(&y2, 2);
emxInit_real_T(&H, 2);
b_emxInit_real_T(&s, 1);
emxInit_real_T(&C, 2);
emxInit_real_T(&b_C, 2);
emxInit_real_T(&c_delta, 2);
emxInit_real_T(&d_D, 2);
b_emxInit_real_T(&b_deltaone, 1);
exitg1 = FALSE;
while ((exitg1 == FALSE) && (i < 500)) {
/* compute gradient, Hessian and search direction (note it is actually */
/* 1/4 of the gradient and Hessian, but the step size is just the ratio */
/* of the gradient and the diagonal of the Hessian so it doesn't matter). */
i0 = delta->size[0] * delta->size[1];
delta->size[0] = dinv->size[0];
delta->size[1] = dinv->size[1];
emxEnsureCapacity((emxArray__common *)delta, i0, (int32_T)sizeof(real_T));
br = dinv->size[0] * dinv->size[1];
for (i0 = 0; i0 < br; i0++) {
delta->data[i0] = dinv->data[i0] - Dinv->data[i0];
}
guard2 = FALSE;
if (delta->size[1] == 1) {
guard2 = TRUE;
} else {
b_D = x->size[0];
if (b_D == 1) {
guard2 = TRUE;
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = deltaone->size[0] * deltaone->size[1];
deltaone->size[0] = (int32_T)unnamed_idx_0;
deltaone->size[1] = 2;
emxEnsureCapacity((emxArray__common *)deltaone, i0, (int32_T)sizeof
(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
deltaone->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
deltaone->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br + 1; ib <= i0; ib++) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
deltaone->data[ic] += delta->data[ia - 1];
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
}
if (guard2 == TRUE) {
i0 = deltaone->size[0] * deltaone->size[1];
deltaone->size[0] = delta->size[0];
deltaone->size[1] = 2;
emxEnsureCapacity((emxArray__common *)deltaone, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
deltaone->data[i0 + deltaone->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
deltaone->data[i0 + deltaone->size[0] * i1] += delta->data[i0 +
delta->size[0] * b_D];
}
}
}
}
if ((delta->size[1] == 1) || (y->size[0] == 1)) {
i0 = g->size[0] * g->size[1];
g->size[0] = delta->size[0];
g->size[1] = 2;
emxEnsureCapacity((emxArray__common *)g, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
g->data[i0 + g->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
g->data[i0 + g->size[0] * i1] += delta->data[i0 + delta->size[0] *
b_D] * y->data[b_D + y->size[0] * i1];
}
}
}
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = g->size[0] * g->size[1];
g->size[0] = (int32_T)unnamed_idx_0;
g->size[1] = 2;
emxEnsureCapacity((emxArray__common *)g, i0, (int32_T)sizeof(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
g->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
g->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br; ib + 1 <= i0; ib++) {
if (y->data[ib] != 0.0) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
g->data[ic] += y->data[ib] * delta->data[ia - 1];
}
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
i0 = g->size[0] * g->size[1];
g->size[1] = 2;
emxEnsureCapacity((emxArray__common *)g, i0, (int32_T)sizeof(real_T));
b_D = g->size[0];
br = g->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
g->data[i0] -= y->data[i0] * deltaone->data[i0];
}
c_power(dinv, delta);
b_power(y, y2);
if ((delta->size[1] == 1) || (y2->size[0] == 1)) {
i0 = H->size[0] * H->size[1];
H->size[0] = delta->size[0];
H->size[1] = 2;
emxEnsureCapacity((emxArray__common *)H, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
H->data[i0 + H->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
H->data[i0 + H->size[0] * i1] += delta->data[i0 + delta->size[0] *
b_D] * y2->data[b_D + y2->size[0] * i1];
}
}
}
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = H->size[0] * H->size[1];
H->size[0] = (int32_T)unnamed_idx_0;
H->size[1] = 2;
emxEnsureCapacity((emxArray__common *)H, i0, (int32_T)sizeof(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
H->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
H->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br; ib + 1 <= i0; ib++) {
if (y2->data[ib] != 0.0) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
H->data[ic] += y2->data[ib] * delta->data[ia - 1];
}
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
if ((delta->size[1] == 1) || (y->size[0] == 1)) {
i0 = C->size[0] * C->size[1];
C->size[0] = delta->size[0];
C->size[1] = 2;
emxEnsureCapacity((emxArray__common *)C, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
C->data[i0 + C->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
C->data[i0 + C->size[0] * i1] += delta->data[i0 + delta->size[0] *
b_D] * y->data[b_D + y->size[0] * i1];
}
}
}
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = C->size[0] * C->size[1];
C->size[0] = (int32_T)unnamed_idx_0;
C->size[1] = 2;
emxEnsureCapacity((emxArray__common *)C, i0, (int32_T)sizeof(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
C->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
C->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br; ib + 1 <= i0; ib++) {
if (y->data[ib] != 0.0) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
C->data[ic] += y->data[ib] * delta->data[ia - 1];
}
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
guard1 = FALSE;
if (delta->size[1] == 1) {
guard1 = TRUE;
} else {
b_D = x->size[0];
if (b_D == 1) {
guard1 = TRUE;
} else {
unnamed_idx_0 = (uint32_T)delta->size[0];
i0 = b_C->size[0] * b_C->size[1];
b_C->size[0] = (int32_T)unnamed_idx_0;
b_C->size[1] = 2;
emxEnsureCapacity((emxArray__common *)b_C, i0, (int32_T)sizeof(real_T));
br = (int32_T)unnamed_idx_0 << 1;
for (i0 = 0; i0 < br; i0++) {
b_C->data[i0] = 0.0;
}
if (delta->size[0] == 0) {
} else {
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
i0 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i0; ic++) {
b_C->data[ic] = 0.0;
}
b_D += delta->size[0];
}
br = 0;
b_D = 0;
while ((delta->size[0] > 0) && (b_D <= delta->size[0])) {
ar = 0;
i0 = br + delta->size[1];
for (ib = br + 1; ib <= i0; ib++) {
ia = ar;
i1 = b_D + delta->size[0];
for (ic = b_D; ic + 1 <= i1; ic++) {
ia++;
b_C->data[ic] += delta->data[ia - 1];
}
ar += delta->size[0];
}
br += delta->size[1];
b_D += delta->size[0];
}
}
}
}
if (guard1 == TRUE) {
i0 = b_C->size[0] * b_C->size[1];
b_C->size[0] = delta->size[0];
b_C->size[1] = 2;
emxEnsureCapacity((emxArray__common *)b_C, i0, (int32_T)sizeof(real_T));
br = delta->size[0];
for (i0 = 0; i0 < br; i0++) {
for (i1 = 0; i1 < 2; i1++) {
b_C->data[i0 + b_C->size[0] * i1] = 0.0;
ar = delta->size[1];
for (b_D = 0; b_D < ar; b_D++) {
b_C->data[i0 + b_C->size[0] * i1] += delta->data[i0 + delta->size[0]
* b_D];
}
}
}
}
i0 = H->size[0] * H->size[1];
H->size[1] = 2;
emxEnsureCapacity((emxArray__common *)H, i0, (int32_T)sizeof(real_T));
b_D = H->size[0];
br = H->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
H->data[i0] = ((H->data[i0] - deltaone->data[i0]) - 2.0 * y->data[i0] *
C->data[i0]) + y2->data[i0] * b_C->data[i0];
}
b_D = H->size[0] << 1;
i0 = s->size[0];
s->size[0] = b_D;
emxEnsureCapacity((emxArray__common *)s, i0, (int32_T)sizeof(real_T));
br = 0;
do {
exitg3 = 0;
b_D = H->size[0] << 1;
if (br <= b_D - 1) {
s->data[br] = fabs(H->data[br]);
br++;
} else {
exitg3 = 1;
}
} while (exitg3 == 0);
i0 = s->size[0];
s->size[0] = g->size[0] << 1;
emxEnsureCapacity((emxArray__common *)s, i0, (int32_T)sizeof(real_T));
br = g->size[0] << 1;
for (i0 = 0; i0 < br; i0++) {
s->data[i0] = -g->data[i0] / s->data[i0];
}
i0 = deltaone->size[0] * deltaone->size[1];
deltaone->size[0] = y->size[0];
deltaone->size[1] = 2;
emxEnsureCapacity((emxArray__common *)deltaone, i0, (int32_T)sizeof(real_T));
br = y->size[0] * y->size[1];
for (i0 = 0; i0 < br; i0++) {
deltaone->data[i0] = y->data[i0];
}
/* use step-halving procedure to ensure progress is made */
ib = 1;
ia = 0;
exitg2 = FALSE;
while ((exitg2 == FALSE) && (ia < 20)) {
ib = ia + 1;
b_D = deltaone->size[0] << 1;
i0 = b_deltaone->size[0];
b_deltaone->size[0] = b_D;
emxEnsureCapacity((emxArray__common *)b_deltaone, i0, (int32_T)sizeof
(real_T));
for (i0 = 0; i0 < b_D; i0++) {
b_deltaone->data[i0] = deltaone->data[i0] + s->data[i0];
}
for (i0 = 0; i0 < 2; i0++) {
b_y[i0] = y->size[i0];
}
i0 = y->size[0] * y->size[1];
y->size[0] = b_y[0];
y->size[1] = b_y[1];
emxEnsureCapacity((emxArray__common *)y, i0, (int32_T)sizeof(real_T));
br = b_y[1];
for (i0 = 0; i0 < br; i0++) {
ar = b_y[0];
for (i1 = 0; i1 < ar; i1++) {
y->data[i1 + y->size[0] * i0] = b_deltaone->data[i1 + b_y[0] * i0];
}
}
b_euclid(y, y, d);
b_D = x->size[0];
i0 = delta->size[0] * delta->size[1];
delta->size[0] = b_D;
emxEnsureCapacity((emxArray__common *)delta, i0, (int32_T)sizeof(real_T));
b_D = x->size[0];
i0 = delta->size[0] * delta->size[1];
delta->size[1] = b_D;
emxEnsureCapacity((emxArray__common *)delta, i0, (int32_T)sizeof(real_T));
br = x->size[0] * x->size[0];
for (i0 = 0; i0 < br; i0++) {
delta->data[i0] = 0.0;
}
E_new = x->size[0];
u1 = x->size[0];
if (E_new <= u1) {
} else {
E_new = u1;
}
b_D = (int32_T)E_new;
if (b_D > 0) {
for (br = 0; br + 1 <= b_D; br++) {
delta->data[br + delta->size[0] * br] = 1.0;
}
}
i0 = d->size[0] * d->size[1];
emxEnsureCapacity((emxArray__common *)d, i0, (int32_T)sizeof(real_T));
b_D = d->size[0];
br = d->size[1];
br *= b_D;
for (i0 = 0; i0 < br; i0++) {
d->data[i0] += delta->data[i0];
}
rdivide(d, dinv);
i0 = d_D->size[0] * d_D->size[1];
d_D->size[0] = D->size[0];
d_D->size[1] = D->size[1];
emxEnsureCapacity((emxArray__common *)d_D, i0, (int32_T)sizeof(real_T));
br = D->size[0] * D->size[1];
for (i0 = 0; i0 < br; i0++) {
d_D->data[i0] = D->data[i0] - d->data[i0];
}
power(d_D, delta);
i0 = c_delta->size[0] * c_delta->size[1];
c_delta->size[0] = delta->size[0];
c_delta->size[1] = delta->size[1];
emxEnsureCapacity((emxArray__common *)c_delta, i0, (int32_T)sizeof(real_T));
br = delta->size[0] * delta->size[1];
for (i0 = 0; i0 < br; i0++) {
c_delta->data[i0] = delta->data[i0] * Dinv->data[i0];
}
c_sum(c_delta, b_x);
if (b_x->size[1] == 0) {
E_new = 0.0;
} else {
E_new = b_x->data[0];
for (br = 2; br <= b_x->size[1]; br++) {
E_new += b_x->data[br - 1];
}
}
if (E_new < E) {
exitg2 = TRUE;
} else {
i0 = s->size[0];
emxEnsureCapacity((emxArray__common *)s, i0, (int32_T)sizeof(real_T));
br = s->size[0];
for (i0 = 0; i0 < br; i0++) {
s->data[i0] *= 0.5;
}
ia++;
}
}
/* bomb out if too many halving steps are required */
if ((ib == 20) || (fabs((E - E_new) / E) < 1.0E-9)) {
exitg1 = TRUE;
} else {
/* evaluate termination criterion */
/* report progress */
E = E_new;
i++;
}
}
emxFree_real_T(&b_deltaone);
emxFree_real_T(&d_D);
emxFree_real_T(&c_delta);
emxFree_real_T(&b_x);
emxFree_real_T(&b_C);
emxFree_real_T(&C);
emxFree_real_T(&s);
emxFree_real_T(&H);
emxFree_real_T(&y2);
emxFree_real_T(&g);
emxFree_real_T(&deltaone);
emxFree_real_T(&delta);
emxFree_real_T(&dinv);
emxFree_real_T(&d);
emxFree_real_T(&Dinv);
emxFree_real_T(&D);
/* fiddle stress to match the original Sammon paper */
}
void sammon(sammonStackData *SD, const real_T x[1000000], real_T y[2000], real_T
*E)
{
real_T B;
int32_T i;
real_T dv0[1000];
int32_T b_i;
boolean_T exitg1;
real_T alpha1;
real_T beta1;
char_T TRANSB;
char_T TRANSA;
ptrdiff_t m_t;
ptrdiff_t n_t;
ptrdiff_t k_t;
ptrdiff_t lda_t;
ptrdiff_t ldb_t;
ptrdiff_t ldc_t;
double * alpha1_t;
double * Aia0_t;
double * Bib0_t;
double * beta1_t;
double * Cic0_t;
real_T g[2000];
real_T y2[2000];
real_T b_y[2000];
real_T c_y[2000];
real_T d_y[2000];
real_T e_y[2000];
real_T b_g[2000];
int32_T j;
int32_T b_j;
boolean_T exitg2;
real_T dv1[1000];
real_T E_new;
/* #codgen */
/* */
/* SAMMON - apply Sammon's nonlinear mapping */
/* */
/* Y = SAMMON(X) applies Sammon's nonlinear mapping procedure on */
/* multivariate data X, where each row represents a pattern and each column */
/* represents a feature. On completion, Y contains the corresponding */
/* co-ordinates of each point on the map. By default, a two-dimensional */
/* map is created. Note if X contains any duplicated rows, SAMMON will */
/* fail (ungracefully). */
/* */
/* [Y,E] = SAMMON(X) also returns the value of the cost function in E (i.e. */
/* the stress of the mapping). */
/* */
/* An N-dimensional output map is generated by Y = SAMMON(X,N) . */
/* */
/* A set of optimisation options can also be specified using a third */
/* argument, Y = SAMMON(X,N,OPTS) , where OPTS is a structure with fields: */
/* */
/* MaxIter - maximum number of iterations */
/* TolFun - relative tolerance on objective function */
/* MaxHalves - maximum number of step halvings */
/* Input - {'raw','distance'} if set to 'distance', X is */
/* interpreted as a matrix of pairwise distances. */
/* Display - {'off', 'on', 'iter'} */
/* Initialisation - {'pca', 'random'} */
/* */
/* The default options structure can be retrieved by calling SAMMON with */
/* no parameters. */
/* */
/* References : */
/* */
/* [1] Sammon, John W. Jr., "A Nonlinear Mapping for Data Structure */
/* Analysis", IEEE Transactions on Computers, vol. C-18, no. 5, */
/* pp 401-409, May 1969. */
/* */
/* See also : SAMMON_TEST */
/* */
/* File : sammon.m */
/* */
/* Date : Monday 12th November 2007. */
/* */
/* Author : Gavin C. Cawley and Nicola L. C. Talbot */
/* */
/* Description : Simple vectorised MATLAB implementation of Sammon's non-linear */
/* mapping algorithm [1]. */
/* */
/* References : [1] Sammon, John W. Jr., "A Nonlinear Mapping for Data */
/* Structure Analysis", IEEE Transactions on Computers, */
/* vol. C-18, no. 5, pp 401-409, May 1969. */
/* */
/* History : 10/08/2004 - v1.00 */
/* 11/08/2004 - v1.10 Hessian made positive semidefinite */
/* 13/08/2004 - v1.11 minor optimisation */
/* 12/11/2007 - v1.20 initialisation using the first n principal */
/* components. */
/* */
/* Thanks : Dr Nick Hamilton ([email protected]) for supplying the */
/* code for implementing initialisation using the first n */
/* principal components (introduced in v1.20). */
/* */
/* To do : The current version does not take advantage of the symmetry */
/* of the distance matrix in order to allow for easy */
/* vectorisation. This may not be a good choice for very large */
/* datasets, so perhaps one day I'll get around to doing a MEX */
/* version using the BLAS library etc. for very large datasets. */
/* */
/* Copyright : (c) Dr Gavin C. Cawley, November 2007. */
/* */
/* This program is free software; you can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published by */
/* the Free Software Foundation; either version 2 of the License, or */
/* (at your option) any later version. */
/* */
/* This program is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */
/* GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public License */
/* along with this program; if not, write to the Free Software */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */
/* */
/* use the default options structure */
/* create distance matrix unless given by parameters */
emlrtPushRtStackR2012b(&emlrtRSI, emlrtRootTLSGlobal);
euclid(SD, x, x, SD->f2.D);
emlrtPopRtStackR2012b(&emlrtRSI, emlrtRootTLSGlobal);
/* remaining initialisation */
B = b_sum(SD->f2.D);
eye(SD->f2.delta);
for (i = 0; i < 1000000; i++) {
SD->f2.D[i] += SD->f2.delta[i];
}
rdivide(1.0, SD->f2.D, SD->f2.Dinv);
emlrtPushRtStackR2012b(&b_emlrtRSI, emlrtRootTLSGlobal);
randn(y);
emlrtPopRtStackR2012b(&b_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&c_emlrtRSI, emlrtRootTLSGlobal);
b_euclid(SD, y, y, SD->f2.d);
eye(SD->f2.delta);
for (i = 0; i < 1000000; i++) {
SD->f2.d[i] += SD->f2.delta[i];
}
emlrtPopRtStackR2012b(&c_emlrtRSI, emlrtRootTLSGlobal);
rdivide(1.0, SD->f2.d, SD->f2.dinv);
for (i = 0; i < 1000000; i++) {
SD->f2.b_D[i] = SD->f2.D[i] - SD->f2.d[i];
}
power(SD->f2.b_D, SD->f2.delta);
for (i = 0; i < 1000000; i++) {
SD->f2.d[i] = SD->f2.delta[i] * SD->f2.Dinv[i];
}
d_sum(SD->f2.d, dv0);
*E = e_sum(dv0);
/* get on with it */
b_i = 0;
exitg1 = FALSE;
while ((exitg1 == FALSE) && (b_i < 500)) {
/* compute gradient, Hessian and search direction (note it is actually */
/* 1/4 of the gradient and Hessian, but the step size is just the ratio */
/* of the gradient and the diagonal of the Hessian so it doesn't matter). */
for (i = 0; i < 1000000; i++) {
SD->f2.delta[i] = SD->f2.dinv[i] - SD->f2.Dinv[i];
}
emlrtPushRtStackR2012b(&d_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
alpha1 = 1.0;
beta1 = 0.0;
TRANSB = 'N';
TRANSA = 'N';
for (i = 0; i < 2000; i++) {
SD->f2.y_old[i] = 1.0;
SD->f2.deltaone[i] = 0.0;
}
emlrtPushRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
m_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
n_t = (ptrdiff_t)(2);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
k_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
lda_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldb_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldc_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
alpha1_t = (double *)(&alpha1);
emlrtPopRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
Aia0_t = (double *)(&SD->f2.delta[0]);
emlrtPopRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
Bib0_t = (double *)(&SD->f2.y_old[0]);
emlrtPopRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
beta1_t = (double *)(&beta1);
emlrtPopRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
Cic0_t = (double *)(&SD->f2.deltaone[0]);
emlrtPopRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
dgemm(&TRANSA, &TRANSB, &m_t, &n_t, &k_t, alpha1_t, Aia0_t, &lda_t, Bib0_t,
&ldb_t, beta1_t, Cic0_t, &ldc_t);
emlrtPopRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&d_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&e_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
alpha1 = 1.0;
beta1 = 0.0;
TRANSB = 'N';
TRANSA = 'N';
memset(&g[0], 0, 2000U * sizeof(real_T));
emlrtPushRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
m_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
n_t = (ptrdiff_t)(2);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
k_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
lda_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldb_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldc_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
alpha1_t = (double *)(&alpha1);
emlrtPopRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
Aia0_t = (double *)(&SD->f2.delta[0]);
emlrtPopRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
Bib0_t = (double *)(&y[0]);
emlrtPopRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
beta1_t = (double *)(&beta1);
emlrtPopRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
Cic0_t = (double *)(&g[0]);
emlrtPopRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
dgemm(&TRANSA, &TRANSB, &m_t, &n_t, &k_t, alpha1_t, Aia0_t, &lda_t, Bib0_t,
&ldb_t, beta1_t, Cic0_t, &ldc_t);
emlrtPopRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
for (i = 0; i < 2000; i++) {
g[i] -= y[i] * SD->f2.deltaone[i];
}
emlrtPopRtStackR2012b(&e_emlrtRSI, emlrtRootTLSGlobal);
c_power(SD->f2.dinv, SD->f2.delta);
b_power(y, y2);
emlrtPushRtStackR2012b(&f_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
alpha1 = 1.0;
beta1 = 0.0;
TRANSB = 'N';
TRANSA = 'N';
memset(&b_y[0], 0, 2000U * sizeof(real_T));
emlrtPushRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
m_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
n_t = (ptrdiff_t)(2);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
k_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
lda_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldb_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldc_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
alpha1_t = (double *)(&alpha1);
emlrtPopRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
Aia0_t = (double *)(&SD->f2.delta[0]);
emlrtPopRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
Bib0_t = (double *)(&y2[0]);
emlrtPopRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
beta1_t = (double *)(&beta1);
emlrtPopRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
Cic0_t = (double *)(&b_y[0]);
emlrtPopRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
dgemm(&TRANSA, &TRANSB, &m_t, &n_t, &k_t, alpha1_t, Aia0_t, &lda_t, Bib0_t,
&ldb_t, beta1_t, Cic0_t, &ldc_t);
emlrtPopRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
for (i = 0; i < 2000; i++) {
c_y[i] = 2.0 * y[i];
}
emlrtPushRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
alpha1 = 1.0;
beta1 = 0.0;
TRANSB = 'N';
TRANSA = 'N';
memset(&d_y[0], 0, 2000U * sizeof(real_T));
emlrtPushRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
m_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
n_t = (ptrdiff_t)(2);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
k_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
lda_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldb_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldc_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
alpha1_t = (double *)(&alpha1);
emlrtPopRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
Aia0_t = (double *)(&SD->f2.delta[0]);
emlrtPopRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
Bib0_t = (double *)(&y[0]);
emlrtPopRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
beta1_t = (double *)(&beta1);
emlrtPopRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
Cic0_t = (double *)(&d_y[0]);
emlrtPopRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
dgemm(&TRANSA, &TRANSB, &m_t, &n_t, &k_t, alpha1_t, Aia0_t, &lda_t, Bib0_t,
&ldb_t, beta1_t, Cic0_t, &ldc_t);
emlrtPopRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
alpha1 = 1.0;
beta1 = 0.0;
TRANSB = 'N';
TRANSA = 'N';
for (i = 0; i < 2000; i++) {
SD->f2.y_old[i] = 1.0;
e_y[i] = 0.0;
}
emlrtPushRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
m_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&l_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
n_t = (ptrdiff_t)(2);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&m_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
k_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&n_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
lda_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&o_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldb_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&p_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
ldc_t = (ptrdiff_t)(1000);
emlrtPopRtStackR2012b(&x_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&q_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
alpha1_t = (double *)(&alpha1);
emlrtPopRtStackR2012b(&r_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
Aia0_t = (double *)(&SD->f2.delta[0]);
emlrtPopRtStackR2012b(&s_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
Bib0_t = (double *)(&SD->f2.y_old[0]);
emlrtPopRtStackR2012b(&t_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
beta1_t = (double *)(&beta1);
emlrtPopRtStackR2012b(&u_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
Cic0_t = (double *)(&e_y[0]);
emlrtPopRtStackR2012b(&v_emlrtRSI, emlrtRootTLSGlobal);
emlrtPushRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
dgemm(&TRANSA, &TRANSB, &m_t, &n_t, &k_t, alpha1_t, Aia0_t, &lda_t, Bib0_t,
&ldb_t, beta1_t, Cic0_t, &ldc_t);
emlrtPopRtStackR2012b(&w_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&k_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&j_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&i_emlrtRSI, emlrtRootTLSGlobal);
emlrtPopRtStackR2012b(&f_emlrtRSI, emlrtRootTLSGlobal);
for (i = 0; i < 2000; i++) {
SD->f2.y_old[i] = ((b_y[i] - SD->f2.deltaone[i]) - c_y[i] * d_y[i]) + y2[i]
* e_y[i];
b_g[i] = -g[i];
}
b_abs(SD->f2.y_old, y2);
for (i = 0; i < 2000; i++) {
SD->f2.deltaone[i] = y2[i];
SD->f2.y_old[i] = y[i];
}
b_rdivide(b_g, SD->f2.deltaone, y2);
/* use step-halving procedure to ensure progress is made */
j = 1;
b_j = 0;
exitg2 = FALSE;
while ((exitg2 == FALSE) && (b_j < 20)) {
j = b_j + 1;
for (i = 0; i < 2000; i++) {
y[i] = SD->f2.y_old[i] + y2[i];
}
emlrtPushRtStackR2012b(&g_emlrtRSI, emlrtRootTLSGlobal);
b_euclid(SD, y, y, SD->f2.d);
eye(SD->f2.delta);
for (i = 0; i < 1000000; i++) {
SD->f2.d[i] += SD->f2.delta[i];
}
emlrtPopRtStackR2012b(&g_emlrtRSI, emlrtRootTLSGlobal);
rdivide(1.0, SD->f2.d, SD->f2.dinv);
for (i = 0; i < 1000000; i++) {
SD->f2.b_D[i] = SD->f2.D[i] - SD->f2.d[i];
}
power(SD->f2.b_D, SD->f2.delta);
for (i = 0; i < 1000000; i++) {
SD->f2.d[i] = SD->f2.delta[i] * SD->f2.Dinv[i];
}
d_sum(SD->f2.d, dv1);
E_new = e_sum(dv1);
if (E_new < *E) {
exitg2 = TRUE;
} else {
for (i = 0; i < 2000; i++) {
y2[i] *= 0.5;
}
b_j++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar,
emlrtRootTLSGlobal);
}
}
/* bomb out if too many halving steps are required */
if ((j == 20) || (muDoubleScalarAbs((*E - E_new) / *E) < 1.0E-9)) {
exitg1 = TRUE;
} else {
/* evaluate termination criterion */
/* report progress */
*E = E_new;
b_i++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, emlrtRootTLSGlobal);
}
}
/* fiddle stress to match the original Sammon paper */
*E *= 0.5 / B;
}
/*
* Arguments : double blat
* double blon
* double ranges
* double azm
* double major_axis
* double esquared
* double *plat
* double *plon
* Return Type : void
*/
void cget_latlon(double blat, double blon, double ranges, double azm, double
major_axis, double esquared, double *plat, double *plon)
{
double c1;
double c2;
double D;
double P;
double ss;
double S;
double phi1;
int ellipse;
double onef;
double f;
double f4;
double al12;
int ii_size_idx_0;
int ii_size_idx_1;
int is1_size_idx_0;
int is1_size_idx_1;
signed char iv0[1];
int loop_ub;
int i0;
signed char iv1[1];
double th1;
double costh1;
double sinth1;
double sina12;
int im1_size_idx_0;
int im1_size_idx_1;
signed char iv2[1];
signed char iv3[1];
double cosa12;
double M;
double N;
double dv0[1];
double tmp_data[1];
double dv1[1];
int tmp_size[2];
double b_tmp_data[1];
int b_tmp_size[2];
double c_tmp_data[1];
double dv2[1];
double dv3[1];
double dv4[1];
double dv5[1];
int c_tmp_size[2];
double dv6[1];
double s1;
double d0;
double d;
double dv7[1];
double u;
double V;
double sind;
double ds;
double cosds;
double sinds;
double dv8[1];
double al21;
double phi2;
double de;
double b_ellipse;
double lam2;
/* CGET_LATLON Get latitudes and longitudes along a line */
/* */
/* function [plat,plon]=cget_latlon(blat,blon,ranges,azm,major_axis); */
/* */
/* This is a translation to matlab of the USGS PROJ-4.4 geographic */
/* projection library to compute positions along a series of ranges */
/* along a given azimuth from a reference point. */
/* Uses ellipsoidal earth model set to Clark 1966 standard. */
/* */
/* Inputs: */
/* blat,blon lat,lon coordinates of start point */
/* in degrees (scalar) */
/* */
/* ranges range in nmi (scalar or vector) */
/* azm single-valued (same size as range) (degrees) */
/* major_axes - skip for an elliptical earth, set=0 for spherical Earth */
/* */
/* Outputs: */
/* plat,plon lat,lons along path */
/* */
c1 = 0.0;
c2 = 0.0;
D = 0.0;
P = 0.0;
ss = 0.0;
if (major_axis == 0.0) {
major_axis = 6.3782064E+6;
esquared = 0.0;
}
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* now do the stuff that used to be in .c */
S = ranges * 1852.0;
phi1 = blat * 0.017453292519943295;
ellipse = (esquared != 0.0);
if (ellipse == 1) {
onef = sqrt(1.0 - esquared);
f = 1.0 - onef;
f4 = (1.0 - onef) / 4.0;
} else {
onef = 1.0;
f = 0.0;
f4 = 0.0;
}
al12 = azm * 0.017453292519943295;
cadjlon(&al12);
if (fabs(al12) > 1.5707963267948966) {
ii_size_idx_0 = 1;
ii_size_idx_1 = 1;
} else {
ii_size_idx_0 = 0;
ii_size_idx_1 = 0;
}
is1_size_idx_0 = ii_size_idx_0;
is1_size_idx_1 = ii_size_idx_1;
iv0[0] = 0;
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
iv0[0] = 1;
i0 = 1;
}
if (fabs(al12) <= 1.5707963267948966) {
ii_size_idx_0 = 1;
ii_size_idx_1 = 1;
} else {
ii_size_idx_0 = 0;
ii_size_idx_1 = 0;
}
iv1[0] = iv0[0];
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
iv1[0] = 0;
i0 = 1;
}
if (ellipse == 1) {
th1 = atan(onef * tan(phi1));
} else {
th1 = phi1;
}
costh1 = cos(th1);
sinth1 = sin(th1);
sina12 = sin(al12);
if (fabs(sina12) < 1.0E-10) {
ii_size_idx_0 = 1;
ii_size_idx_1 = 1;
} else {
ii_size_idx_0 = 0;
ii_size_idx_1 = 0;
}
im1_size_idx_0 = ii_size_idx_0;
im1_size_idx_1 = ii_size_idx_1;
iv2[0] = 0;
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
iv2[0] = 1;
i0 = 1;
}
if (fabs(sina12) >= 1.0E-10) {
ii_size_idx_0 = 1;
ii_size_idx_1 = 1;
} else {
ii_size_idx_0 = 0;
ii_size_idx_1 = 0;
}
iv3[0] = iv2[0];
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
iv3[0] = 0;
i0 = 1;
}
if (iv3[0] == 1) {
sina12 = 0.0;
if (fabs(al12) < 1.5707963267948966) {
cosa12 = 1.0;
} else {
cosa12 = -1.0;
}
M = 0.0;
} else {
cosa12 = cos(al12);
M = costh1 * sina12;
}
N = costh1 * cosa12;
if (ellipse == 1) {
/* if merid(j) == 1 */
dv0[0] = 0.0;
loop_ub = im1_size_idx_0 * im1_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
dv0[0] = f4;
i0 = 1;
}
tmp_data[0] = 0.0;
loop_ub = im1_size_idx_0 * im1_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
tmp_data[0] = 1.0 - dv0[0];
i0 = 1;
}
dv1[0] = tmp_data[0];
loop_ub = im1_size_idx_0 * im1_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
dv1[0] = tmp_data[0] * tmp_data[0];
i0 = 1;
}
tmp_size[0] = im1_size_idx_0;
tmp_size[1] = im1_size_idx_1;
loop_ub = im1_size_idx_0 * im1_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
tmp_data[0] = dv0[0];
i0 = 1;
}
loop_ub = im1_size_idx_0 * im1_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
b_tmp_data[0] = dv1[0];
i0 = 1;
}
b_rdivide(tmp_data, tmp_size, b_tmp_data, c_tmp_data, b_tmp_size);
dv2[0] = 0.0;
loop_ub = b_tmp_size[0] * b_tmp_size[1];
for (i0 = 0; i0 < loop_ub; i0++) {
dv2[0] = c_tmp_data[i0];
}
/* else */
dv3[0] = 0.0;
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
dv3[0] = f * M;
i0 = 1;
}
c1 = dv3[0];
dv4[0] = dv0[0];
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
dv4[0] = f4 * (1.0 - M * M);
i0 = 1;
}
c2 = dv4[0];
dv5[0] = dv1[0];
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
dv5[0] = (1.0 - dv4[0]) * ((1.0 - dv4[0]) - dv3[0] * M);
i0 = 1;
}
D = dv5[0];
c_tmp_size[0] = ii_size_idx_0;
c_tmp_size[1] = ii_size_idx_1;
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
tmp_data[0] = (1.0 + 0.5 * dv3[0] * M) * dv4[0];
i0 = 1;
}
loop_ub = ii_size_idx_0 * ii_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
b_tmp_data[0] = dv5[0];
i0 = 1;
}
b_rdivide(tmp_data, c_tmp_size, b_tmp_data, c_tmp_data, b_tmp_size);
dv6[0] = dv2[0];
loop_ub = b_tmp_size[0] * b_tmp_size[1];
for (i0 = 0; i0 < loop_ub; i0++) {
dv6[0] = c_tmp_data[i0];
}
P = dv6[0];
/* end */
}
if (iv3[0] == 1) {
s1 = 1.5707963267948966 - th1;
} else {
if (fabs(M) >= 1.0) {
d0 = 0.0;
} else {
d0 = acos(M);
}
s1 = sinth1 / sin(d0);
if (fabs(s1) >= 1.0) {
s1 = 0.0;
} else {
s1 = acos(s1);
}
}
if (ellipse == 1) {
d = rdivide(S, D * major_axis);
dv7[0] = d;
loop_ub = is1_size_idx_0 * is1_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
dv7[0] = -d;
i0 = 1;
}
u = 2.0 * (s1 - dv7[0]);
V = cos(u + dv7[0]);
sind = sin(dv7[0]);
ds = (dv7[0] + c2 * c2 * sind * cos(dv7[0]) * (2.0 * V * V - 1.0)) - 2.0 * P
* V * (1.0 - 2.0 * P * cos(u)) * sind;
ss = (s1 + s1) - ds;
} else {
ds = S / major_axis;
dv7[0] = ds;
loop_ub = is1_size_idx_0 * is1_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
dv7[0] = -ds;
i0 = 1;
}
ds = dv7[0];
}
cosds = cos(ds);
sinds = sin(ds);
dv8[0] = sinds;
loop_ub = is1_size_idx_0 * is1_size_idx_1;
i0 = 0;
while (i0 <= loop_ub - 1) {
dv8[0] = -sinds;
i0 = 1;
}
al21 = N * cosds - sinth1 * dv8[0];
if (iv3[0] == 1) {
phi2 = atan(tan((1.5707963267948966 + s1) - ds) / onef);
if (al21 > 0.0) {
if (iv1[0] == 1) {
de = 3.1415926535897931;
} else {
phi2 = -phi2;
de = 0.0;
}
} else if (iv1[0] == 1) {
phi2 = -phi2;
de = 0.0;
} else {
de = 3.1415926535897931;
}
} else {
al21 = atan(M / al21);
if (al21 > 0.0) {
al21 += 3.1415926535897931;
}
if (al12 < 0.0) {
al21 -= 3.1415926535897931;
}
cadjlon(&al21);
if (ellipse == 1) {
b_ellipse = onef * M;
} else {
b_ellipse = M;
}
phi2 = atan(-(sinth1 * cosds + N * dv8[0]) * sin(al21) / b_ellipse);
de = rt_atan2d_snf(dv8[0] * sina12, costh1 * cosds - sinth1 * dv8[0] *
cosa12);
if (ellipse == 1) {
if (iv1[0] == 1) {
de += c1 * ((1.0 - c2) * ds + c2 * dv8[0] * cos(ss));
} else {
de -= c1 * ((1.0 - c2) * ds - c2 * dv8[0] * cos(ss));
}
}
}
lam2 = blon * 0.017453292519943295 + de;
cadjlon(&lam2);
*plon = lam2 * 57.295779513082323;
*plat = phi2 * 57.295779513082323;
}
void drr_SK_Simplified(const emxArray_boolean_T *voxel_data, const real_T
voxel_size_mm[3], const real_T detector_dimensions[2], real_T pixel_size_mm,
const real_T voxel_corner_min[3], const real_T T_carm[16], emxArray_real_T
*projection)
{
int32_T ix;
int32_T i;
uint32_T voxDim[3];
real_T voxPlanes__max[3];
real_T source[3];
real_T pixel_size_mmel__hn[3];
real_T pixel_size_mmel__wn[3];
real_T corner[3];
real_T b_ix;
real_T iy;
int32_T ih;
int32_T iw;
real_T tnext[3];
real_T tstep[3];
real_T b_voxPlanes__max[3];
real_T ray_source2pixel[3];
real_T b_ray_source2pixel[3];
real_T pixel_point_mm[3];
real_T t_plane_min[3];
real_T t_plane_max[3];
boolean_T exitg8;
boolean_T exitg7;
real_T iz;
boolean_T exitg6;
real_T t_larger[4];
boolean_T exitg5;
boolean_T exitg4;
boolean_T exitg3;
real_T t_smaller[4];
real_T temp;
int32_T itmp;
boolean_T exitg2;
real_T tmax;
boolean_T exitg1;
real_T tx;
real_T ty;
real_T tz;
/* function [projection] = drr (voxel_data, voxel_size_mm, detector_dimensions, pixel_size_mm, isocenter_mm, cbct_angles_deg) */
/* Creates a 2D projection at each cbct_angle of voxel_data. the projection */
/* axis is defined by the isocenter to which the source and center of */
/* the detector are aligned. This simulation assumes standard Cone Beam CT */
/* geometry (source to isocenter distance is 100 cm and source to detector */
/* distance is 150cm). */
/* */
/* voxel_data: must be a 3 dimensional matrix (typically of CT data) */
/* voxel_size_mm: a 3 element vector listing the size (in mm) of the voxels */
/* along each dimension */
/* detector_dimension: a 2 element vector listing the dimensions (number of */
/* pixels) in each dimension (u,v) */
/* pixel_size_mm: a number defining the height and width of each pixel */
/* (assumes square pixel) */
/* isocenter_mm: a 3 element vector pointing from the origin (corner) of the */
/* matrix element(1,1,1) to the isocenter */
/* cbct_angles_deg: a list of angles to generate projections */
/* */
/* Retrun Variable */
/* projection: a 3D matrix with the 3rd dimension representing the angle of */
/* roatation */
/* */
/* { */
/* Author: Michael M. Folkerts [email protected] */
/* Institution: UCSD Physics, UTSW Radiation Oncology */
/* Updated: 2014-July. */
/* Notes: Siddon's Incremental algorithm | modified to read in 3D matlab matrix | Simplified Input | No guarantees!! */
/* */
/* References: */
/* R.L. Siddon, */
/* "Fast calculation of the exact radiological path for a three-dimensional CT */
/* array," Medical Physics 12, 252-255 (1985). */
/* */
/* F. Jacobs, E. Sundermann, B. De Sutter, M. Christiaens and I. Lemahieu, */
/* "A fast algorithm to calculate the exact radiological path through a pixel_size_mmel or voxel space," */
/* Journal of Computing and Information Technology 6, 89-94 (1998). */
/* */
/* G. Han, Z. Liang and J. You, */
/* "A fast ray tracing technique for TCT and ECT studies," */
/* IEEE Medical Imaging Conference 1999. */
/* } */
/* if(0) */
/* voxel_data = OUTPUTgrid; */
/* voxel_size_mm = voxel_size; */
/* detector_dimensions = detector_dimension; */
/* pixel_size_mm = pixel_size; */
/* isocenter_mm = isocenter; */
/* T_carm: Transformation matrix of C-arm (that is set at the middle of */
/* detector & source) with respect to Voxel coordinates */
/* tic; */
/* this will verify the size: */
if (eml_ndims_varsized(*(int32_T (*)[3])voxel_data->size) != 3) {
EMLRTPUSHRTSTACK(&emlrtRSI);
error();
EMLRTPOPRTSTACK(&emlrtRSI);
}
/* constants: */
/* .0001; */
/* .0001; */
/* sounce to imager(detector) distance */
/* source to axis distance %% RRI set the coordinate at the middle between source and the detector */
/* initialize memory for speed: */
ix = projection->size[0] * projection->size[1];
projection->size[0] = (int32_T)emlrtIntegerCheckR2009b
(emlrtNonNegativeCheckR2009b(detector_dimensions[0], &b_emlrtDCI), &emlrtDCI);
projection->size[1] = (int32_T)emlrtIntegerCheckR2009b
(emlrtNonNegativeCheckR2009b(detector_dimensions[1], &d_emlrtDCI),
&c_emlrtDCI);
emxEnsureCapacity((emxArray__common *)projection, ix, (int32_T)sizeof(real_T),
&emlrtRTEI);
i = (int32_T)emlrtIntegerCheckR2009b(emlrtNonNegativeCheckR2009b
(detector_dimensions[0], &f_emlrtDCI), &e_emlrtDCI) * (int32_T)
emlrtIntegerCheckR2009b(emlrtNonNegativeCheckR2009b(detector_dimensions[1],
&h_emlrtDCI), &g_emlrtDCI) - 1;
for (ix = 0; ix <= i; ix++) {
projection->data[ix] = 0.0;
}
for (ix = 0; ix < 3; ix++) {
voxDim[ix] = (uint32_T)voxel_data->size[ix];
}
/* [size(voxel_data,1), size(voxel_data,2), size(voxel_data,3)]; */
/* difine voxel boundaries: */
/* vector from origin to source */
/* vector from origin to CENTER of detector */
/* define pixel_size_mmel vectors: */
/* define upper lefthand corner of detector: */
for (i = 0; i < 3; i++) {
voxPlanes__max[i] = voxel_corner_min[i] + voxel_size_mm[i] * (real_T)
voxDim[i];
/* define up: */
/* up = [0,0,1]; */
/* width of projection image */
/* height of projection image */
/* direction from the detector to the source */
/* end initialization timer: */
/* init_time = toc */
/* start tracing timer: */
/* tic; */
source[i] = 550.0 * T_carm[4 + i] + T_carm[12 + i];
b_ix = -pixel_size_mm * T_carm[i];
/* length of pixel_size_mmel */
iy = pixel_size_mm * T_carm[8 + i];
/* vector pointing left, parallel to detector */
/* define incremental vectors: */
pixel_size_mmel__hn[i] = -iy;
pixel_size_mmel__wn[i] = -b_ix;
corner[i] = (-550.0 * T_carm[4 + i] + T_carm[12 + i]) +
(detector_dimensions[0] * b_ix + detector_dimensions[1] * iy) / 2.0;
}
emlrtForLoopVectorCheckR2011b(1.0, 1.0, detector_dimensions[0], mxDOUBLE_CLASS,
(int32_T)detector_dimensions[0], &d_emlrtRTEI,
&emlrtContextGlobal, 0);
ih = 0;
while (ih <= (int32_T)detector_dimensions[0] - 1) {
/* if(mod(ih,100)==0),fprintf('height:\t%i...\n',ih);end */
emlrtForLoopVectorCheckR2011b(1.0, 1.0, detector_dimensions[1],
mxDOUBLE_CLASS, (int32_T)detector_dimensions[1], &c_emlrtRTEI,
&emlrtContextGlobal, 0);
iw = 0;
while (iw <= (int32_T)detector_dimensions[1] - 1) {
/* ray to be traced */
/* find parametrized (t) voxel plane (min or max) intersections: */
/* PLANE = P1 + t(P2-P1) */
/* SK added */
/* t_x = (i-x1) / (x2-x1), t_y = (j-y1) / (y2-y1), t_z = (k-z1) / (z2-z1) */
for (i = 0; i < 3; i++) {
b_ix = (corner[i] + ((1.0 + (real_T)ih) - 1.0) * pixel_size_mmel__hn[i])
+ ((1.0 + (real_T)iw) - 1.0) * pixel_size_mmel__wn[i];
/* ray end point */
iy = b_ix - source[i];
tnext[i] = voxel_corner_min[i] - source[i];
tstep[i] = iy + 2.2250738585072014E-308;
b_voxPlanes__max[i] = voxPlanes__max[i] - source[i];
ray_source2pixel[i] = iy + 2.2250738585072014E-308;
b_ray_source2pixel[i] = iy;
pixel_point_mm[i] = b_ix;
}
rdivide(tnext, tstep, t_plane_min);
rdivide(b_voxPlanes__max, ray_source2pixel, t_plane_max);
/* compute (parametric) intersection values */
/* tmin = max { min{tx(0), tx(Nx)}, min{ty(0), ty(Ny)], min{tz(0), tz(Nz)}, 0.0 } */
/* tmax = min { max{tx(0), tx(Nx)}, max{ty(0), ty(Ny)], max{tz(0), tz(Nz)}, 1.0 } */
/* t_larger = [ max([t_plane_min(1), t_plane_max(1)]), max([t_plane_min(2), t_plane_max(2)]), max([t_plane_min(3), t_plane_max(3)]), 1.0 ]; */
/* t_smaller = [ min([t_plane_min(1), t_plane_max(1)]), min([t_plane_min(2), t_plane_max(2)]), min([t_plane_min(3), t_plane_max(3)]), 0.0 ]; */
i = 1;
b_ix = t_plane_min[0];
if (muDoubleScalarIsNaN(t_plane_min[0])) {
ix = 2;
exitg8 = FALSE;
while ((exitg8 == 0U) && (ix < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[0])) {
b_ix = t_plane_max[0];
exitg8 = TRUE;
} else {
ix = 3;
}
}
}
if ((i < 2) && (t_plane_max[0] > b_ix)) {
b_ix = t_plane_max[0];
}
i = 1;
iy = t_plane_min[1];
if (muDoubleScalarIsNaN(t_plane_min[1])) {
ix = 2;
exitg7 = FALSE;
while ((exitg7 == 0U) && (ix < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[1])) {
iy = t_plane_max[1];
exitg7 = TRUE;
} else {
ix = 3;
}
}
}
if ((i < 2) && (t_plane_max[1] > iy)) {
iy = t_plane_max[1];
}
i = 1;
iz = t_plane_min[2];
if (muDoubleScalarIsNaN(t_plane_min[2])) {
ix = 2;
exitg6 = FALSE;
while ((exitg6 == 0U) && (ix < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[2])) {
iz = t_plane_max[2];
exitg6 = TRUE;
} else {
ix = 3;
}
}
}
if ((i < 2) && (t_plane_max[2] > iz)) {
iz = t_plane_max[2];
}
t_larger[0] = b_ix;
t_larger[1] = iy;
t_larger[2] = iz;
t_larger[3] = 1.0;
i = 1;
b_ix = t_plane_min[0];
if (muDoubleScalarIsNaN(t_plane_min[0])) {
ix = 2;
exitg5 = FALSE;
while ((exitg5 == 0U) && (ix < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[0])) {
b_ix = t_plane_max[0];
exitg5 = TRUE;
} else {
ix = 3;
}
}
}
if ((i < 2) && (t_plane_max[0] < b_ix)) {
b_ix = t_plane_max[0];
}
i = 1;
iy = t_plane_min[1];
if (muDoubleScalarIsNaN(t_plane_min[1])) {
ix = 2;
exitg4 = FALSE;
while ((exitg4 == 0U) && (ix < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[1])) {
iy = t_plane_max[1];
exitg4 = TRUE;
} else {
ix = 3;
}
}
}
if ((i < 2) && (t_plane_max[1] < iy)) {
iy = t_plane_max[1];
}
i = 1;
iz = t_plane_min[2];
if (muDoubleScalarIsNaN(t_plane_min[2])) {
ix = 2;
exitg3 = FALSE;
while ((exitg3 == 0U) && (ix < 3)) {
i = 2;
if (!muDoubleScalarIsNaN(t_plane_max[2])) {
iz = t_plane_max[2];
exitg3 = TRUE;
} else {
ix = 3;
}
}
}
if ((i < 2) && (t_plane_max[2] < iz)) {
iz = t_plane_max[2];
}
t_smaller[0] = b_ix;
t_smaller[1] = iy;
t_smaller[2] = iz;
t_smaller[3] = 0.0;
i = 1;
temp = t_smaller[0];
itmp = 0;
if (muDoubleScalarIsNaN(t_smaller[0])) {
ix = 2;
exitg2 = FALSE;
while ((exitg2 == 0U) && (ix < 5)) {
i = ix;
if (!muDoubleScalarIsNaN(t_smaller[ix - 1])) {
temp = t_smaller[ix - 1];
exitg2 = TRUE;
} else {
ix++;
}
}
}
if (i < 4) {
while (i + 1 < 5) {
if (t_smaller[i] > temp) {
temp = t_smaller[i];
itmp = i;
}
i++;
}
}
i = 1;
tmax = t_larger[0];
if (muDoubleScalarIsNaN(t_larger[0])) {
ix = 2;
exitg1 = FALSE;
while ((exitg1 == 0U) && (ix < 5)) {
i = ix;
if (!muDoubleScalarIsNaN(t_larger[ix - 1])) {
tmax = t_larger[ix - 1];
exitg1 = TRUE;
} else {
ix++;
}
}
}
if (i < 4) {
while (i + 1 < 5) {
if (t_larger[i] < tmax) {
tmax = t_larger[i];
}
i++;
}
}
for (ix = 0; ix < 3; ix++) {
pixel_point_mm[ix] = (real_T)(pixel_point_mm[ix] < source[ix]) * -2.0 +
1.0;
}
if (temp < tmax) {
/* if ray intersects volume */
/* find index for each dimension: */
for (i = 0; i < 3; i++) {
b_ix = muDoubleScalarFloor((((b_ray_source2pixel[i] * temp + source[i])
- voxel_corner_min[i]) + 2.2250738585072014E-308) / voxel_size_mm[i]);
/* (parametric) intersection values... */
/* makes 0 or 1 */
tnext[i] = (voxel_corner_min[i] + ((b_ix + (pixel_point_mm[i] + 1.0) /
2.0) * voxel_size_mm[i] - source[i])) / (b_ray_source2pixel[i] +
2.2250738585072014E-308);
/* parametric value for next plane intersection */
iy = voxel_size_mm[i] / (b_ray_source2pixel[i] +
2.2250738585072014E-308);
tstep[i] = muDoubleScalarAbs(iy);
b_ray_source2pixel[i] = iy;
t_plane_min[i] = b_ix;
}
/* parametric step size */
/* address special cases... */
if (temp == t_plane_max[emlrtBoundsCheck(itmp + 1, &c_emlrtBCI) - 1]) {
/* if intersection is a "max" plane */
if (itmp + 1 == 1) {
t_plane_min[0] = (real_T)voxDim[0] - 1.0;
} else if (itmp + 1 == 2) {
t_plane_min[1] = (real_T)voxDim[1] - 2.0;
} else {
t_plane_min[2] = (real_T)voxDim[2] - 2.0;
}
tnext[itmp] = temp + tstep[itmp];
/* next plane crossing */
} else {
t_plane_min[itmp] = 0.0;
tnext[itmp] = temp + tstep[itmp];
/* next plane crossing */
}
/* voxel index values(add one for matlab): */
b_ix = t_plane_min[0] + 1.0;
iy = t_plane_min[1] + 1.0;
iz = t_plane_min[2] + 1.0;
tx = tnext[0];
ty = tnext[1];
tz = tnext[2];
i = 0;
/* uncomment to generate P-matrix: */
/* pixel_size_mmNum = 1; */
/* len = norm(ray_source2pixel); % ray length */
while ((temp + 0.0001 < tmax) && (!(b_ix * iy * iz == 0.0))) {
if ((tx < ty) && (tx < tz)) {
/* 실제 DRR을 그리지 않고, 한점이라도 지나면 해당 점에 포함시킴 */
if (voxel_data->data[((emlrtDynamicBoundsCheck((int32_T)
emlrtIntegerCheckR2009b(b_ix, &o_emlrtDCI), 1,
voxel_data->size[0], &j_emlrtBCI) + voxel_data->size[0] *
(emlrtDynamicBoundsCheck((int32_T)
emlrtIntegerCheckR2009b(iy, &p_emlrtDCI), 1,
voxel_data->size[1], &k_emlrtBCI) - 1)) + voxel_data->size[0]
* voxel_data->size[1] *
(emlrtDynamicBoundsCheck((int32_T)
emlrtIntegerCheckR2009b(iz, &q_emlrtDCI), 1, voxel_data->
size[2], &l_emlrtBCI) - 1)) - 1]) {
i = 255;
tmax = rtMinusInf;
}
temp = tx;
tx += tstep[0];
b_ix += pixel_point_mm[0];
} else if (ty < tz) {
/* 실제 DRR을 그리지 않고, 한점이라도 지나면 해당 점에 포함시킴 */
if (voxel_data->data[((emlrtDynamicBoundsCheck((int32_T)
emlrtIntegerCheckR2009b(b_ix, &l_emlrtDCI), 1,
voxel_data->size[0], &g_emlrtBCI) + voxel_data->size[0] *
(emlrtDynamicBoundsCheck((int32_T)
emlrtIntegerCheckR2009b(iy, &m_emlrtDCI), 1,
voxel_data->size[1], &h_emlrtBCI) - 1)) + voxel_data->size[0]
* voxel_data->size[1] *
(emlrtDynamicBoundsCheck((int32_T)
emlrtIntegerCheckR2009b(iz, &n_emlrtDCI), 1, voxel_data->
size[2], &i_emlrtBCI) - 1)) - 1]) {
i = 255;
tmax = rtMinusInf;
}
temp = ty;
ty += tstep[1];
iy += pixel_point_mm[1];
} else {
/* 실제 DRR을 그리지 않고, 한점이라도 지나면 해당 점에 포함시킴 */
if (voxel_data->data[((emlrtDynamicBoundsCheck((int32_T)
emlrtIntegerCheckR2009b(b_ix, &i_emlrtDCI), 1,
voxel_data->size[0], &d_emlrtBCI) + voxel_data->size[0] *
(emlrtDynamicBoundsCheck((int32_T)
emlrtIntegerCheckR2009b(iy, &j_emlrtDCI), 1,
voxel_data->size[1], &e_emlrtBCI) - 1)) + voxel_data->size[0]
* voxel_data->size[1] *
(emlrtDynamicBoundsCheck((int32_T)
emlrtIntegerCheckR2009b(iz, &k_emlrtDCI), 1, voxel_data->
size[2], &f_emlrtBCI) - 1)) - 1]) {
i = 255;
tmax = rtMinusInf;
}
temp = tz;
tz += tstep[2];
iz += pixel_point_mm[2];
}
emlrtBreakCheck();
}
/* end while */
projection->data[(emlrtDynamicBoundsCheck((int32_T)(1.0 + (real_T)ih), 1,
projection->size[0], &m_emlrtBCI) + projection->size[0] *
(emlrtDynamicBoundsCheck((int32_T)(1.0 + (real_T)iw),
1, projection->size[1], &n_emlrtBCI) - 1)) - 1] = (real_T)i;
} else {
/* if no intersections */
projection->data[(emlrtDynamicBoundsCheck((int32_T)(1.0 + (real_T)ih), 1,
projection->size[0], &emlrtBCI) + projection->size[0] *
(emlrtDynamicBoundsCheck((int32_T)(1.0 + (real_T)iw),
1, projection->size[1], &b_emlrtBCI) - 1)) - 1] = 0.0;
}
/* if intersections */
/* uncomment to generate P-matrix: */
/* rayCount = rayCount + 1; */
iw++;
emlrtBreakCheck();
}
/* width */
/* fprintf('\n'); */
ih++;
emlrtBreakCheck();
}
/* height */
/* uncomment to generate P-matrix: */
/* matrix = matrix(1:mtxCount-1,:); */
/* stop trace timer: */
/* trace_time = toc */
/* fprintf('\n') */
/* function */
/* } */
}
void b_Acoeff(const emlrtStack *sp, real_T ksi, real_T j, const emxArray_real_T *
x, real_T t, const emxArray_real_T *gridT, emxArray_real_T *vals)
{
emxArray_real_T *b;
emxArray_real_T *r8;
int32_T b_x;
int32_T i;
emxArray_boolean_T *b_t;
real_T c_x;
emxArray_boolean_T *c_t;
emxArray_real_T *z0;
emxArray_real_T *d_x;
emxArray_real_T *e_x;
emxArray_real_T *r9;
int32_T b_b[2];
int32_T f_x[2];
emxArray_real_T *g_x;
emxArray_real_T *r10;
const mxArray *y;
static const int32_T iv16[2] = { 1, 45 };
const mxArray *m6;
char_T cv18[45];
static const char_T cv19[45] = { 'C', 'o', 'd', 'e', 'r', ':', 't', 'o', 'o',
'l', 'b', 'o', 'x', ':', 'm', 't', 'i', 'm', 'e', 's', '_', 'n', 'o', 'D',
'y', 'n', 'a', 'm', 'i', 'c', 'S', 'c', 'a', 'l', 'a', 'r', 'E', 'x', 'p',
'a', 'n', 's', 'i', 'o', 'n' };
const mxArray *b_y;
static const int32_T iv17[2] = { 1, 21 };
char_T cv20[21];
static const char_T cv21[21] = { 'C', 'o', 'd', 'e', 'r', ':', 'M', 'A', 'T',
'L', 'A', 'B', ':', 'i', 'n', 'n', 'e', 'r', 'd', 'i', 'm' };
emxArray_boolean_T *d_t;
real_T h_x;
emxArray_boolean_T *e_t;
emxArray_real_T *i_x;
emxArray_real_T *r11;
emxArray_real_T *j_x;
emxArray_real_T *r12;
emxArray_real_T *z1;
int32_T b_z0[2];
emxArray_real_T *c_z0;
emxArray_real_T *k_x;
const mxArray *c_y;
static const int32_T iv18[2] = { 1, 45 };
const mxArray *d_y;
static const int32_T iv19[2] = { 1, 21 };
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &b_st;
d_st.tls = b_st.tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
emxInit_real_T(sp, &b, 2, &bb_emlrtRTEI, true);
emxInit_real_T(sp, &r8, 2, &bb_emlrtRTEI, true);
/* evaluate the coefficient A at the boundary ksi=0 or ksi=1; */
/* for the index j which describes the time steps timePoints_j, at time t and space */
/* point x */
/* timePoints is a vector describing the time descritized domain */
b_x = gridT->size[1];
i = (int32_T)emlrtIntegerCheckFastR2012b(j, &emlrtDCI, sp);
if (t <= gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &d_emlrtBCI,
sp) - 1]) {
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof(real_T),
&bb_emlrtRTEI);
vals->data[0] = 0.0;
} else {
emxInit_boolean_T(sp, &b_t, 2, &bb_emlrtRTEI, true);
b_x = b_t->size[0] * b_t->size[1];
b_t->size[0] = 1;
b_t->size[1] = 2 + x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)b_t, b_x, (int32_T)sizeof
(boolean_T), &bb_emlrtRTEI);
b_x = gridT->size[1];
i = (int32_T)j;
b_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x,
&e_emlrtBCI, sp) - 1]);
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
b_t->data[b_t->size[0]] = (t <= gridT->
data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &f_emlrtBCI, sp) - 1]);
i = x->size[1];
for (b_x = 0; b_x < i; b_x++) {
b_t->data[b_t->size[0] * (b_x + 2)] = (x->data[x->size[0] * b_x] == ksi);
}
st.site = &pe_emlrtRSI;
if (all(&st, b_t)) {
b_x = gridT->size[1];
i = (int32_T)j;
emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &c_emlrtBCI, sp);
c_x = (t - gridT->data[(int32_T)j - 1]) / 3.1415926535897931;
st.site = &emlrtRSI;
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
vals->data[0] = muDoubleScalarSqrt(c_x);
} else {
emxInit_boolean_T(sp, &c_t, 2, &bb_emlrtRTEI, true);
b_x = c_t->size[0] * c_t->size[1];
c_t->size[0] = 1;
c_t->size[1] = 2 + x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)c_t, b_x, (int32_T)sizeof
(boolean_T), &bb_emlrtRTEI);
b_x = gridT->size[1];
i = (int32_T)j;
c_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1,
b_x, &g_emlrtBCI, sp) - 1]);
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
c_t->data[c_t->size[0]] = (t <= gridT->
data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &h_emlrtBCI, sp) - 1]);
i = x->size[1];
for (b_x = 0; b_x < i; b_x++) {
c_t->data[c_t->size[0] * (b_x + 2)] = (x->data[x->size[0] * b_x] != ksi);
}
emxInit_real_T(sp, &z0, 2, &cb_emlrtRTEI, true);
emxInit_real_T(sp, &d_x, 2, &bb_emlrtRTEI, true);
st.site = &qe_emlrtRSI;
if (all(&st, c_t)) {
st.site = &b_emlrtRSI;
b_x = gridT->size[1];
i = (int32_T)j;
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x,
&m_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
emxInit_real_T(&st, &e_x, 2, &bb_emlrtRTEI, true);
b_x = e_x->size[0] * e_x->size[1];
e_x->size[0] = 1;
e_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)e_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
e_x->data[b_x] = x->data[b_x] - ksi;
}
emxInit_real_T(sp, &r9, 2, &bb_emlrtRTEI, true);
b_abs(sp, e_x, r9);
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = r9->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = r9->size[0] * r9->size[1];
emxFree_real_T(&e_x);
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = r9->data[b_x];
}
emxFree_real_T(&r9);
rdivide(sp, r8, 2.0 * muDoubleScalarSqrt(c_x), z0);
st.site = &re_emlrtRSI;
mpower(&st, z0, d_x);
b_x = d_x->size[0] * d_x->size[1];
d_x->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0];
b_x = d_x->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
d_x->data[b_x] = -d_x->data[b_x];
}
b_x = b->size[0] * b->size[1];
b->size[0] = 1;
b->size[1] = d_x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)b, b_x, (int32_T)sizeof(real_T),
&bb_emlrtRTEI);
i = d_x->size[0] * d_x->size[1];
for (b_x = 0; b_x < i; b_x++) {
b->data[b_x] = d_x->data[b_x];
}
for (i = 0; i < d_x->size[1]; i++) {
b->data[i] = muDoubleScalarExp(b->data[i]);
}
st.site = &re_emlrtRSI;
b_mrdivide(&st, b, z0);
for (b_x = 0; b_x < 2; b_x++) {
i = d_x->size[0] * d_x->size[1];
d_x->size[b_x] = z0->size[b_x];
emxEnsureCapacity(sp, (emxArray__common *)d_x, i, (int32_T)sizeof
(real_T), &ab_emlrtRTEI);
}
for (i = 0; i < z0->size[1]; i++) {
d_x->data[i] = scalar_erf(z0->data[i]);
}
b_x = d_x->size[0] * d_x->size[1];
d_x->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0];
b_x = d_x->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
d_x->data[b_x] *= 1.7724538509055159;
}
for (b_x = 0; b_x < 2; b_x++) {
b_b[b_x] = b->size[b_x];
}
for (b_x = 0; b_x < 2; b_x++) {
f_x[b_x] = d_x->size[b_x];
}
emxInit_real_T(sp, &g_x, 2, &bb_emlrtRTEI, true);
emlrtSizeEqCheck2DFastR2012b(b_b, f_x, &o_emlrtECI, sp);
b_x = g_x->size[0] * g_x->size[1];
g_x->size[0] = 1;
g_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)g_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
g_x->data[b_x] = x->data[b_x] - ksi;
}
emxInit_real_T(sp, &r10, 2, &bb_emlrtRTEI, true);
b_abs(sp, g_x, r10);
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = r10->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = r10->size[0] * r10->size[1];
emxFree_real_T(&g_x);
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = r10->data[b_x];
}
emxFree_real_T(&r10);
rdivide(sp, r8, 3.5449077018110318, vals);
st.site = &re_emlrtRSI;
b_x = b->size[0] * b->size[1];
b->size[0] = 1;
emxEnsureCapacity(&st, (emxArray__common *)b, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = b->size[0];
b_x = b->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
b->data[b_x] -= d_x->data[b_x];
}
b_st.site = &he_emlrtRSI;
if (!(vals->size[1] == 1)) {
if ((vals->size[1] == 1) || (b->size[1] == 1)) {
y = NULL;
m6 = emlrtCreateCharArray(2, iv16);
for (i = 0; i < 45; i++) {
cv18[i] = cv19[i];
}
emlrtInitCharArrayR2013a(&b_st, 45, m6, cv18);
emlrtAssign(&y, m6);
c_st.site = &fh_emlrtRSI;
d_st.site = &vg_emlrtRSI;
b_error(&c_st, message(&d_st, y, &j_emlrtMCI), &k_emlrtMCI);
} else {
b_y = NULL;
m6 = emlrtCreateCharArray(2, iv17);
for (i = 0; i < 21; i++) {
cv20[i] = cv21[i];
}
emlrtInitCharArrayR2013a(&b_st, 21, m6, cv20);
emlrtAssign(&b_y, m6);
c_st.site = &gh_emlrtRSI;
d_st.site = &wg_emlrtRSI;
b_error(&c_st, message(&d_st, b_y, &l_emlrtMCI), &m_emlrtMCI);
}
}
c_x = vals->data[0];
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = b->size[1];
emxEnsureCapacity(&st, (emxArray__common *)vals, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = b->size[1];
for (b_x = 0; b_x < i; b_x++) {
vals->data[vals->size[0] * b_x] = c_x * b->data[b->size[0] * b_x];
}
} else {
emxInit_boolean_T(sp, &d_t, 2, &bb_emlrtRTEI, true);
b_x = d_t->size[0] * d_t->size[1];
d_t->size[0] = 1;
d_t->size[1] = 1 + x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)d_t, b_x, (int32_T)sizeof
(boolean_T), &bb_emlrtRTEI);
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
d_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1,
b_x, &i_emlrtBCI, sp) - 1]);
i = x->size[1];
for (b_x = 0; b_x < i; b_x++) {
d_t->data[d_t->size[0] * (b_x + 1)] = (x->data[x->size[0] * b_x] ==
ksi);
}
st.site = &se_emlrtRSI;
if (all(&st, d_t)) {
b_x = gridT->size[1];
i = (int32_T)j;
emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &b_emlrtBCI, sp);
c_x = (t - gridT->data[(int32_T)j - 1]) / 3.1415926535897931;
b_x = gridT->size[1];
i = (int32_T)(j + 1.0);
emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &emlrtBCI, sp);
h_x = (t - gridT->data[(int32_T)(j + 1.0) - 1]) / 3.1415926535897931;
st.site = &c_emlrtRSI;
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
st.site = &c_emlrtRSI;
if (h_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
vals->data[0] = muDoubleScalarSqrt(c_x) - muDoubleScalarSqrt(h_x);
} else {
emxInit_boolean_T(sp, &e_t, 2, &bb_emlrtRTEI, true);
b_x = e_t->size[0] * e_t->size[1];
e_t->size[0] = 1;
e_t->size[1] = 1 + x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)e_t, b_x, (int32_T)sizeof
(boolean_T), &bb_emlrtRTEI);
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
e_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1,
b_x, &j_emlrtBCI, sp) - 1]);
i = x->size[1];
for (b_x = 0; b_x < i; b_x++) {
e_t->data[e_t->size[0] * (b_x + 1)] = (x->data[x->size[0] * b_x] !=
ksi);
}
st.site = &te_emlrtRSI;
if (all(&st, e_t)) {
st.site = &d_emlrtRSI;
b_x = gridT->size[1];
i = (int32_T)j;
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x,
&k_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
emxInit_real_T(&st, &i_x, 2, &bb_emlrtRTEI, true);
b_x = i_x->size[0] * i_x->size[1];
i_x->size[0] = 1;
i_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)i_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
i_x->data[b_x] = x->data[b_x] - ksi;
}
emxInit_real_T(sp, &r11, 2, &bb_emlrtRTEI, true);
b_abs(sp, i_x, r11);
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = r11->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = r11->size[0] * r11->size[1];
emxFree_real_T(&i_x);
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = r11->data[b_x];
}
emxFree_real_T(&r11);
rdivide(sp, r8, 2.0 * muDoubleScalarSqrt(c_x), z0);
st.site = &e_emlrtRSI;
b_x = gridT->size[1];
i = (int32_T)((uint32_T)j + 1U);
c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x,
&l_emlrtBCI, &st) - 1];
if (c_x < 0.0) {
b_st.site = &f_emlrtRSI;
eml_error(&b_st);
}
emxInit_real_T(&st, &j_x, 2, &bb_emlrtRTEI, true);
b_x = j_x->size[0] * j_x->size[1];
j_x->size[0] = 1;
j_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)j_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
j_x->data[b_x] = x->data[b_x] - ksi;
}
emxInit_real_T(sp, &r12, 2, &bb_emlrtRTEI, true);
b_abs(sp, j_x, r12);
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = r12->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = r12->size[0] * r12->size[1];
emxFree_real_T(&j_x);
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = r12->data[b_x];
}
emxFree_real_T(&r12);
emxInit_real_T(sp, &z1, 2, &db_emlrtRTEI, true);
rdivide(sp, r8, 2.0 * muDoubleScalarSqrt(c_x), z1);
st.site = &ue_emlrtRSI;
mpower(&st, z0, d_x);
b_x = d_x->size[0] * d_x->size[1];
d_x->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0];
b_x = d_x->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
d_x->data[b_x] = -d_x->data[b_x];
}
b_x = b->size[0] * b->size[1];
b->size[0] = 1;
b->size[1] = d_x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)b, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0] * d_x->size[1];
for (b_x = 0; b_x < i; b_x++) {
b->data[b_x] = d_x->data[b_x];
}
for (i = 0; i < d_x->size[1]; i++) {
b->data[i] = muDoubleScalarExp(b->data[i]);
}
st.site = &ue_emlrtRSI;
b_mrdivide(&st, b, z0);
st.site = &ue_emlrtRSI;
mpower(&st, z1, d_x);
b_x = d_x->size[0] * d_x->size[1];
d_x->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0];
b_x = d_x->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
d_x->data[b_x] = -d_x->data[b_x];
}
b_x = r8->size[0] * r8->size[1];
r8->size[0] = 1;
r8->size[1] = d_x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = d_x->size[0] * d_x->size[1];
for (b_x = 0; b_x < i; b_x++) {
r8->data[b_x] = d_x->data[b_x];
}
for (i = 0; i < d_x->size[1]; i++) {
r8->data[i] = muDoubleScalarExp(r8->data[i]);
}
st.site = &ue_emlrtRSI;
b_mrdivide(&st, r8, z1);
for (b_x = 0; b_x < 2; b_x++) {
b_b[b_x] = b->size[b_x];
}
for (b_x = 0; b_x < 2; b_x++) {
b_z0[b_x] = r8->size[b_x];
}
emxInit_real_T(sp, &c_z0, 2, &bb_emlrtRTEI, true);
emlrtSizeEqCheck2DFastR2012b(b_b, b_z0, &m_emlrtECI, sp);
b_x = c_z0->size[0] * c_z0->size[1];
c_z0->size[0] = 1;
c_z0->size[1] = z0->size[1];
emxEnsureCapacity(sp, (emxArray__common *)c_z0, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = z0->size[0] * z0->size[1];
for (b_x = 0; b_x < i; b_x++) {
c_z0->data[b_x] = z0->data[b_x];
}
b_erf(sp, c_z0, z0);
b_erf(sp, z1, d_x);
emxFree_real_T(&c_z0);
emxFree_real_T(&z1);
for (b_x = 0; b_x < 2; b_x++) {
b_z0[b_x] = z0->size[b_x];
}
for (b_x = 0; b_x < 2; b_x++) {
f_x[b_x] = d_x->size[b_x];
}
emlrtSizeEqCheck2DFastR2012b(b_z0, f_x, &n_emlrtECI, sp);
b_x = z0->size[0] * z0->size[1];
z0->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)z0, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = z0->size[0];
b_x = z0->size[1];
i *= b_x;
for (b_x = 0; b_x < i; b_x++) {
z0->data[b_x] = 1.7724538509055159 * (z0->data[b_x] - d_x->
data[b_x]);
}
for (b_x = 0; b_x < 2; b_x++) {
b_b[b_x] = b->size[b_x];
}
for (b_x = 0; b_x < 2; b_x++) {
b_z0[b_x] = z0->size[b_x];
}
emxInit_real_T(sp, &k_x, 2, &bb_emlrtRTEI, true);
emlrtSizeEqCheck2DFastR2012b(b_b, b_z0, &m_emlrtECI, sp);
b_x = k_x->size[0] * k_x->size[1];
k_x->size[0] = 1;
k_x->size[1] = x->size[1];
emxEnsureCapacity(sp, (emxArray__common *)k_x, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = x->size[0] * x->size[1];
for (b_x = 0; b_x < i; b_x++) {
k_x->data[b_x] = x->data[b_x] - ksi;
}
b_abs(sp, k_x, d_x);
rdivide(sp, d_x, 3.5449077018110318, vals);
st.site = &ue_emlrtRSI;
b_x = b->size[0] * b->size[1];
b->size[0] = 1;
emxEnsureCapacity(&st, (emxArray__common *)b, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
i = b->size[0];
b_x = b->size[1];
i *= b_x;
emxFree_real_T(&k_x);
for (b_x = 0; b_x < i; b_x++) {
b->data[b_x] = (b->data[b_x] - r8->data[b_x]) + z0->data[b_x];
}
b_st.site = &he_emlrtRSI;
if (!(vals->size[1] == 1)) {
if ((vals->size[1] == 1) || (b->size[1] == 1)) {
c_y = NULL;
m6 = emlrtCreateCharArray(2, iv18);
for (i = 0; i < 45; i++) {
cv18[i] = cv19[i];
}
emlrtInitCharArrayR2013a(&b_st, 45, m6, cv18);
emlrtAssign(&c_y, m6);
c_st.site = &fh_emlrtRSI;
d_st.site = &vg_emlrtRSI;
b_error(&c_st, message(&d_st, c_y, &j_emlrtMCI), &k_emlrtMCI);
} else {
d_y = NULL;
m6 = emlrtCreateCharArray(2, iv19);
for (i = 0; i < 21; i++) {
cv20[i] = cv21[i];
}
emlrtInitCharArrayR2013a(&b_st, 21, m6, cv20);
emlrtAssign(&d_y, m6);
c_st.site = &gh_emlrtRSI;
d_st.site = &wg_emlrtRSI;
b_error(&c_st, message(&d_st, d_y, &l_emlrtMCI), &m_emlrtMCI);
}
}
c_x = vals->data[0];
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = b->size[1];
emxEnsureCapacity(&st, (emxArray__common *)vals, b_x, (int32_T)
sizeof(real_T), &bb_emlrtRTEI);
i = b->size[1];
for (b_x = 0; b_x < i; b_x++) {
vals->data[vals->size[0] * b_x] = c_x * b->data[b->size[0] * b_x];
}
} else {
b_x = vals->size[0] * vals->size[1];
vals->size[0] = 1;
vals->size[1] = 1;
emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof
(real_T), &bb_emlrtRTEI);
vals->data[0] = 0.0;
}
emxFree_boolean_T(&e_t);
}
emxFree_boolean_T(&d_t);
}
emxFree_boolean_T(&c_t);
emxFree_real_T(&d_x);
emxFree_real_T(&z0);
}
emxFree_boolean_T(&b_t);
}
emxFree_real_T(&r8);
emxFree_real_T(&b);
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
/* Function Definitions */
void GetThermalPropertyHCVaporReal(double p, double T, double MW, double T_CR,
double p_CR, double W, double *rho_v, double *Cp_v, double *Cv_v, double
*lambda)
{
double p_R;
double T_R;
double v_R_ideal;
double TT[5];
int i13;
double d14;
int i;
static const double b[4] = { 0.1181193, -0.265728, -0.15479, -0.030323 };
double b_TT[3];
double d15;
static const double b_b[3] = { 0.0236744, -0.0186984, 0.0 };
double D;
double d16;
static const double c_b[4] = { 0.2026579, -0.331511, -0.027655, -0.203488 };
double d17;
static const double d_b[3] = { 0.0313385, -0.0503618, 0.016901 };
double D_h;
double VV[6];
double VV_h[6];
double V_R_error;
double V_R;
double c43;
int j;
double c41;
double y;
double varargin_1;
double V_R_temp;
double z0;
double V_R_h;
double c41_h;
double c43_h;
static const double e_b[5] = { -0.077548, 4.5022582138, -1.70019347497797,
0.282256664878756, -0.0122278061244889 };
double dprdTr;
double C_v_pe0;
double Cp_pe0;
double dprdTr_h;
double C_v_peh;
double L;
double p_R4;
double T_r5;
double T_r9;
double T_r16;
double C_v_p;
double expo;
/* The function calculates the density, isobaric and isochronic specific heat */
/* capacity of the real hydrocarbon gases and thermal conductivity based on */
/* the %procudre 7A.1, 7D3.6, 7E1.6, 12B1.5 and 12B4.1 in API Technical Book */
/* of Petroleum Refining. */
/* Input */
/* p : pressure in Pa */
/* T : Temperature in K */
/* MW : Molecular weight in kg/kmol */
/* T_CR : Critical temperature in K */
/* p_CR : Critical pressure in Pa */
/* W : Acentric factor */
/* Output */
/* rho_v : Density of real gas in kg/m3 */
/* Cp_v : Isobaric specific heat capacity of the real gas in J/kg/K */
/* Cv_v : Isochoric specific heat capacity of the real gas in J/kg/K */
/* lambda : Thermal conductivity of the vapor in W/mK */
/* Reference */
/* API Technical Data Book - Petroleum Refining(1985) */
/* */
/* Created by Kevin Koosup Yum, 3 September 2013 */
/* % Consants */
/* Gas constant in J/kmolK */
p_R = p / p_CR;
T_R = T / T_CR;
v_R_ideal = rdivide(8314.0 * T, p) / (8314.0 * T_CR / p_CR);
for (i13 = 0; i13 < 5; i13++) {
TT[i13] = 0.0;
}
TT[0] = 1.0;
d14 = 0.0;
for (i = 0; i < 4; i++) {
TT[i + 1] = TT[i] * T_R;
d14 += 1.0 / TT[i] * b[i];
}
for (i13 = 0; i13 < 2; i13++) {
b_TT[i13] = TT[i13];
}
b_TT[2] = TT[3];
d15 = 0.0;
for (i13 = 0; i13 < 3; i13++) {
d15 += 1.0 / b_TT[i13] * b_b[i13];
}
D = 1.55488E-5 + rdivide(6.23689E-5, T_R);
d16 = 0.0;
for (i13 = 0; i13 < 4; i13++) {
d16 += 1.0 / TT[i13] * c_b[i13];
}
for (i13 = 0; i13 < 2; i13++) {
b_TT[i13] = TT[i13];
}
b_TT[2] = TT[3];
d17 = 0.0;
for (i13 = 0; i13 < 3; i13++) {
d17 += 1.0 / b_TT[i13] * d_b[i13];
}
D_h = 4.8736E-5 + rdivide(7.40336E-6, T_R);
for (i = 0; i < 6; i++) {
VV[i] = 0.0;
VV_h[i] = 0.0;
}
/* % Calculate the density of vapor */
/* Simple fluid calculation */
V_R_error = 1.0;
V_R = v_R_ideal * 0.8;
c43 = 0.0;
while (V_R_error > 0.001) {
VV[0] = V_R;
for (j = 0; j < 5; j++) {
VV[j + 1] = VV[j] * V_R;
}
c41 = 0.042724 / TT[3] / VV[1];
y = 0.060167 / VV[1];
c43 = exp(-0.060167 / VV[1]);
varargin_1 = 0.1 * V_R;
y = V_R - (p_R / T_R * V_R - ((((1.0 + d14 / V_R) + d15 / VV[1]) + D / VV[4])
+ c41 * (0.65392 + y) * c43)) / (p_R / T_R - (((((-d14 / VV[1] -
2.0 * d15 / VV[2]) - 5.0 * D / VV[5]) + -0.085448 / TT[3] / VV[2] *
(0.65392 + y) * c43) + c41 * (-0.120334 / VV[2]) * c43) + c41 * (0.65392 +
y) * (0.120334 / VV[2] * c43)));
if ((varargin_1 >= y) || rtIsNaN(y)) {
V_R_temp = varargin_1;
} else {
V_R_temp = y;
}
V_R_error = fabs(V_R_temp - V_R) / V_R;
V_R = V_R_temp;
}
z0 = p_R * V_R / T_R;
/* Heavy Reference fluid calculation */
V_R_h = v_R_ideal * 0.8;
V_R_error = 1.0;
while (V_R_error > 0.001) {
VV_h[0] = V_R_h;
for (j = 0; j < 5; j++) {
VV_h[j + 1] = VV_h[j] * V_R_h;
}
c41_h = 0.041577 / TT[3] / VV_h[1];
y = 0.03754 / VV_h[1];
c43_h = exp(-0.03754 / VV_h[1]);
varargin_1 = 0.1 * V_R_h;
y = V_R_h - (p_R / T_R * V_R_h - ((((1.0 + d16 / V_R_h) + d17 / VV_h[1]) +
D_h / VV_h[4]) + c41_h * (1.226 + y) * c43_h)) / (p_R / T_R - (((((-d16 /
VV_h[1] - 2.0 * d17 / VV_h[2]) - 5.0 * D_h / VV_h[5]) + -0.083154 / TT[3] /
VV_h[2] * (1.226 + y) * c43_h) + c41_h * (-0.07508 / VV_h[2]) * c43_h) +
c41_h * (1.226 + y) * (0.07508 / VV_h[2] * c43)));
if ((varargin_1 >= y) || rtIsNaN(y)) {
V_R_temp = varargin_1;
} else {
V_R_temp = y;
}
V_R_error = fabs(V_R_temp - V_R_h) / V_R_h;
V_R_h = V_R_temp;
}
*rho_v = 1.0 / ((z0 + W / 0.3978 * (p_R * V_R_h / T_R - z0)) * 8314.0 * T / p)
* MW;
/* % Calculate the CP and Cv */
y = 0.0;
for (i13 = 0; i13 < 5; i13++) {
y += TT[i13] * e_b[i13];
}
dprdTr = rdivide(1.0, VV[0]) * (1.0 + ((((0.1181193 + (-0.15479 * TT[1] +
-0.060646) / TT[3]) * VV[3] + (0.0236744 - 0.0 / TT[3]) * VV[2]) +
1.55488E-5) - 0.085448 / TT[3] * VV[2] * ((0.65392 + 0.060167 / VV[1]) * exp
(-0.060167 / VV[1]))) / VV[4]);
C_v_pe0 = (2.0 * (-0.15479 + -0.090969 / TT[1]) / TT[2] / VV[0] + 0.0 / TT[3] /
VV[1]) + 6.0 * (0.042724 / (2.0 * TT[3] * 0.060167) *
(1.6539199999999998 - (1.6539199999999998 + 0.060167 / VV[1]) * exp
(-0.060167 / VV[1])));
Cp_pe0 = (1.0 + T_R * (dprdTr * dprdTr) / (-TT[1] / VV[1] * (1.0 + (((2.0 *
d14 * VV[3] + 3.0 * d15 * VV[2]) + 6.0 * D) + 0.042724 / TT[3] * VV[2] *
(1.96176 + (5.0 - 2.0 * (0.65392 + 0.060167 / VV[1])) * 0.060167 / VV[1]) *
exp(-0.060167 / VV[1])) / VV[4]))) + C_v_pe0;
dprdTr_h = 1.0 / VV_h[0] * (1.0 + ((((0.2026579 + (-0.027655 * TT[1] +
-0.406976) / TT[3]) * VV_h[3] + (0.0313385 - 0.033802 / TT[3]) * VV_h[2]) +
4.8736E-5) - 0.083154 / TT[3] * VV_h[2] * ((1.226 + 0.03754 / VV_h[1]) * exp
(-0.03754 / VV_h[1]))) / VV_h[4]);
C_v_peh = (2.0 * (-0.027655 + -0.610464 / TT[1]) / TT[2] / VV_h[0] + 0.050703 /
TT[3] / VV_h[1]) + 6.0 * (0.041577 / (2.0 * TT[3] * 0.03754) *
(2.226 - (2.226 + 0.03754 / VV_h[1]) * exp(-0.03754 / VV_h[1])));
*Cp_v = y - 8.314 / MW * (Cp_pe0 + W / 0.3978 * (((1.0 + T_R * (dprdTr_h *
dprdTr_h) / (-TT[1] / VV_h[1] * (1.0 + (((2.0 * d16 * VV_h[3] + 3.0 * d17 *
VV_h[2]) + 6.0 * D_h) + 0.041577 / TT[3] * VV_h[2] * (3.678 + (5.0 - 2.0 *
(1.226 + 0.03754 / VV_h[1])) * 0.03754 / VV_h[1]) * exp(-0.03754 / VV_h[1]))
/ VV_h[4]))) + C_v_peh) - Cp_pe0));
/* in kJ/kgK */
*Cv_v = y - 8.314 / MW * (1.0 + (C_v_pe0 + W / 0.3978 * (C_v_peh - C_v_pe0)));
/* in kJ/kgK */
/* % Calculate Thermal Conductivity */
L = 23.9712 * rt_powd_snf(T_CR, 0.1667) * sqrt(MW) / rt_powd_snf(p_CR / 1000.0,
0.6667);
if (T_R < 1.0) {
*lambda = 0.0004911 * T_R * *Cp_v * MW / L;
} else {
*lambda = 0.0001104 * rt_powd_snf(14.52 * T_R - 5.14, 0.6667) * *Cp_v * MW /
L;
}
if (p > 345000.0) {
p_R4 = rt_powd_snf(p_R, 4.0);
/* p_r5 = p_r4*p_r; */
T_r5 = TT[4] * T_R;
T_r9 = T_r5 * TT[4];
T_r16 = T_r9 * T_r5 * TT[2];
C_v_p = 20.79 - 8.314 * (1.0 + C_v_pe0);
expo = exp(-0.0617 * exp(1.91 / T_r9) * rt_powd_snf(p_R, 2.29 * exp(1.34 /
T_r16)));
/* noncyclic compound */
*lambda *= (C_v_p * (1.0 + (4.18 + (0.537 * p_R * T_R * (1.0 - expo) + 0.51 *
p_R * expo) * T_R) / TT[4]) + (*Cv_v * MW - C_v_p) * (1.0 + (p_R4 / (2.44 *
(T_r16 * TT[4]) + p_R4) + 0.012 * p_R * TT[4]) / T_r5)) / (*Cv_v * MW);
}
*Cp_v *= 1000.0;
*Cv_v *= 1000.0;
}
/////////////////////////////////////////////////////////////////////////////
// Implementation when type is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::lcm_, tag::cpu_,
(A0)(X),
((simd_<arithmetic_<A0>,X>))
((simd_<arithmetic_<A0>,X>))
);
namespace nt2 { namespace ext
{
template<class X, class Dummy>
struct call<tag::lcm_(tag::simd_<tag::arithmetic_, X> ,
tag::simd_<tag::arithmetic_, X> ),
tag::cpu_, Dummy> : callable
{
template<class Sig> struct result;
template<class This,class A0>
struct result<This(A0,A0)>
: meta::strip<A0>{};//
NT2_FUNCTOR_CALL(2)
{
return nt2::abs(round2even(a0)*rdivide(round2even(a1), gcd(a0,a1)));
}
};
} }
#endif
// modified by jt the 05/01/2011
