int main(int argc, char **argv)
{
int i, j, n=50;
double *A, *B, *C, time;
/* Extracción de argumentos */
if (argc == 2) { /* El usuario ha indicado el valor de n */
if ((n = atoi(argv[1])) < 0) n = 50;
}
/* Creación de las matrices */
A = (double*)malloc(n*n*sizeof(double));
B = (double*)malloc(n*n*sizeof(double));
C = (double*)malloc(n*n*sizeof(double));
/* Inicializar matrices */
for(i=0; i<n; i++) {
for(j=0; j<n; j++) {
A[i+n*j] = drand48();
B[i+n*j] = drand48();
}
}
/* Multiplicación de matrices */
time = omp_get_wtime();
matmat(n,A,B,C);
time = omp_get_wtime() - time;
printf("%d;%d;%f\n", omp_get_max_threads(), n, time);
free(A);
free(B);
free(C);
return 0;
}
void hshrowmat(int lr, int ur, int lc, int uc, int i,
real_t x, real_t **u, real_t **a)
{
real_t matmat(int, int, int, int, real_t **, real_t **);
void elmcolrow(int, int, int, int, real_t **, real_t **, real_t);
for (; lc<=uc; lc++) elmcolrow(lr,ur,lc,i,a,u,matmat(lr,ur,i,lc,u,a)*x);
}
void Ti_Optimization::psttfmmat(double **a, int n, double **v, double b[])
{
/*double matmat(int, int, int, int, double **, double **);
void elmcol(int, int, int, int, double **, double **, double);*/
int i,i1,j;
double h;
i1=n;
v[n][n]=1.0;
for (i=n-1; i>=1; i--) {
h=b[i]*a[i][i1];
if (h < 0.0) {
for (j=i1; j<=n; j++) v[j][i]=a[i][j]/h;
for (j=i1; j<=n; j++)
elmcol(i1,n,j,i,v,v,matmat(i1,n,i,j,a,v));
}
for (j=i1; j<=n; j++) v[i][j]=v[j][i]=0.0;
v[i][i]=1.0;
i1=i;
}
}
int main(int argc, char **argv)
{
int i, j, n=50;
double *A, *B, *C, time;
struct timeval t0, t1;
/* Extracción de argumentos */
if (argc == 2) { /* El usuario ha indicado el valor de n */
if ((n = atoi(argv[1])) < 0) n = 50;
}
/* Creación de las matrices */
A = (double*)malloc(n*n*sizeof(double));
B = (double*)malloc(n*n*sizeof(double));
C = (double*)malloc(n*n*sizeof(double));
/* Inicializar matrices */
for(i=0; i<n; i++) {
for(j=0; j<n; j++) {
A[i+n*j] = drand48();
B[i+n*j] = drand48();
}
}
/* Multiplicación de matrices */
gettimeofday (&t0, NULL);
matmat(n,A,B,C);
gettimeofday (&t1, NULL);
printf("1;%f\n",(t1.tv_sec-t0.tv_sec)+(t1.tv_usec-t0.tv_usec)/1000000.0);
free(A);
free(B);
free(C);
return 0;
}
void semip_gc(
//Output Arguments
double *bdraw, // draws for beta (ndraw - nomit,k) matrix
double *adraw, // draws for regional effects, a, (m,1) vector
double *pdraw, // draws for rho (ndraw - nomit,1) vector
double *sdraw, // draws for sige (ndraw - nomit,1) vector
double *rdraw, // draws for rval (ndraw - nomit,1) vector (if mm != 0)
double *vmean, // mean of vi draws (n,1) vector
double *amean, // mean of a-draws (m,1) vector
double *zmean, // mean of latent z-draws (n,1) vector
double *yhat, // mean of posterior predicted y (n,1) vector
//Input Arguments
double *y, // (n,1) lhs vector with 0,1 values
double *x, // (n,k) matrix of explanatory variables
double *W, // (m,m) weight matrix
int ndraw, // # of draws
int nomit, // # of burn-in draws to omit
int nsave, // # of draws saved (= ndraw - nomit)
int n, // # of observations
int k, // # of explanatory variables
int m, // # of regions
int *mobs, // (m,1) vector of obs numbers in each region
double *a, // (m,1) vector of regional effects
double nu, // prior parameter for sige
double d0, // prior parameter for sige
double rval, // hyperparameter r
double mm, // prior parameter for rval
double kk, // prior parameter for rval
double *detval, // (ngrid,2) matrix with [rho , log det values]
int ngrid, // # of values in detval (rows)
double *TI, // prior var-cov for beta (inverted in matlab)
double *TIc) // prior var-cov * prior mean
{
// Local Variables
int i,j,l,iter,invt,accept,rcount,cobs,obsi,zflag;
double rmin,rmax,sige,chi,ee,ratio,p,rho,rho_new;
double junk,aBBa,vsqrt,ru,rhox,rhoy,phi,awi,aw2i,bi,di;
double *z,*tvec,*e,*e0,*xb,*mn,*ys,*limit1,*limit2,*zmt;
double *bhat,*b0,*b0tmp,*v,*vv,*b1,*b1tmp,*Bpa,*rdet,*ldet,*w1;
double **xs,**xst,**xsxs,**A0,**A1,**Bpt,**Bp,**xmat,**W2;
double *Wa, **Wmat;
double c0, c1,c2;
// Allocate Matrices and Vectors
xs = dmatrix(0,n-1,0,k-1);
xst = dmatrix(0,k-1,0,n-1);
xsxs = dmatrix(0,k-1,0,k-1);
A0 = dmatrix(0,k-1,0,k-1);
A1 = dmatrix(0,m-1,0,m-1);
Bp = dmatrix(0,m-1,0,m-1);
Bpt = dmatrix(0,m-1,0,m-1);
xmat = dmatrix(0,n-1,0,k-1);
W2 = dmatrix(0,m-1,0,m-1);
Wmat = dmatrix(0,m-1,0,m-1);
z = dvector(0,n-1);
tvec = dvector(0,n-1);
e = dvector(0,n-1);
e0 = dvector(0,n-1);
xb = dvector(0,n-1);
mn = dvector(0,n-1);
ys = dvector(0,n-1);
limit1 = dvector(0,n-1);
limit2 = dvector(0,n-1);
zmt = dvector(0,n-1);
vv = dvector(0,n-1);
bhat = dvector(0,k-1);
b0 = dvector(0,k-1);
b0tmp = dvector(0,k-1);
v = dvector(0,m-1);
b1 = dvector(0,m-1);
b1tmp = dvector(0,m-1);
Bpa = dvector(0,m-1);
w1 = dvector(0,m-1);
rdet = dvector(0,ngrid-1);
ldet = dvector(0,ngrid-1);
Wa = dvector(0,m-1);
// Initializations
junk = 0.0; // Placeholder for mean in meanvar()
rho = 0.5;
sige = 1.0;
zflag = 0; // a flag for 0,1 y-values
// Initialize (z,vv,limits,tvec)
for(i=0; i<n; i++){
z[i] = y[i];
vv[i] = 1.0;
tvec[i] = 1.0;
if (y[i] == 0.0){
limit1[i] = -10000;
limit2[i] = 0.0;
zflag = 1; // we have 0,1 y-values so sample z
}else{
limit1[i] = 0.0;
limit2[i] = 10000;
}
}
// Initialize v, b1tmp, Bp
for(i=0; i<m; i++){
v[i] = 1.0;
b1tmp[i] = 1.0;
for(j=0; j<m; j++){
if (j == i){
Bp[i][j] = 1 - rho*W[i + j*m];
}else{
Bp[i][j] = - rho*W[i + j*m];
}
Wmat[i][j] = W[i + j*m];
}
}
// Parse detval into rdet and ldet vectors
// and define (rmin,rmax)
for(i=0; i<ngrid; i++){
j=0;
rdet[i] = detval[i + j*ngrid];
j=1;
ldet[i] = detval[i + j*ngrid];
}
rmin = rdet[0];
rmax = rdet[ngrid-1];
// Put x into xmat
for(i=0; i<n; i++){
for(j=0; j<k; j++)
xmat[i][j] = x[i + j*n];
}
// Compute matrices to be used for updating a-vector
if(m > 100){ // Used only for large m
for(i=0; i<m; i++){
w1[i] = 0.0; // Form w1[i]
for(j=0;j<m;j++){
w1[i] = w1[i] + (W[j + i*m]*W[j + i*m]);
}
for(j=0;j<m;j++){ // Form W2[i][*]
W2[i][j] = 0.0;
if(j != i){
for(l=0;l<m;l++){
W2[i][j] = W2[i][j] + W[l + i*m]*W[l + j*m];
}
} // Note that W2[i][i] = 0.0 by construction
}
}
} // End if(m > 10)
// ======================================
// Start the Sampler
// ======================================
for(iter=0; iter<ndraw; iter++){
// UPDATE: beta
// (1) Apply stnd devs (vsqrt) to x, z, z - tvec
for(i=0; i<n; i++){
vsqrt = sqrt(vv[i]);
zmt[i] = (z[i] - tvec[i])/vsqrt;
for(j=0; j<k; j++)
xs[i][j] = x[i + j*n]/vsqrt;
}
// (2) Construct A0 matrix
transpose(xs,n,k,xst); // form xs'
matmat(xst,k,n,xs,k,xsxs); // form xs'*xs
for(i=0; i<k; i++){ // form xs'*xs + TI
for(j=0; j<k; j++)
A0[i][j] = xsxs[i][j] + TI[i + j*k];
}
invt = inverse(A0, k); // replace A0 = inv(A0)
if (invt != 1)
mexPrintf("semip_gc: Inversion error in beta conditional \n");
// (3) Construct b0 vector
matvec(xst,k,n,zmt,b0tmp); //form xs'*zmt
for(i=0; i<k; i++){
b0tmp[i] = b0tmp[i] + TIc[i]; //form b0tmp = xs'*zmt + TIc
}
matvec(A0,k,k,b0tmp,b0); //form b0 = A0*b0tmp
// (4) Do multivariate normal draw from N(b0,A0)
normal_rndc(A0, k, bhat); // generate N(0,A0) vector
for(i=0; i<k; i++){
bhat[i] = bhat[i] + b0[i]; // add mean vector, b0
}
// (5) Now update related values:
matvec(xmat,n,k,bhat,xb); // form xb = xmat*bhat
for(i=0; i<n; i++){ // form e0 = z - x*bhat
e0[i] = z[i] - xb[i];
}
cobs = 0;
for(i=0; i<m; i++){ // form b1tmp = e0i/v[i]
obsi = mobs[i];
b1tmp[i] = 0.0;
for(j=cobs; j<cobs + obsi; j++){
b1tmp[i] = b1tmp[i] + (e0[j]/v[i]);
}
cobs = cobs + obsi;
}
// UPDATE: a
if(m <= 100){ // For small m use straight inverse
// (1) Define A1 and b1
transpose(Bp,m,m,Bpt); // form Bp'
matmat(Bpt,m,m,Bp,m,A1); // form Bp'*Bp
for(i=0; i<m; i++){ // form A1 = (1/sige)*Bp'Bp + diag(mobs/v)
for(j=0; j<m; j++){
if (j == i){
A1[i][j] = (1/sige)*A1[i][j] + ((double)mobs[i])/v[i];
}else{
A1[i][j] = (1/sige)*A1[i][j];
}
}
}
inverse(A1,m); // set A1 = inv(A1)
matvec(A1,m,m,b1tmp,b1); // form b1
// (2) Do multivariate normal draw from N(b1,A1)
normal_rndc(A1,m,a); // generate N(0,A1) vector
for(i=0; i<m; i++){
a[i] = a[i] + b1[i]; // add mean vector, b1
}
}else{ // For large m use marginal distributions
cobs = 0;
for(i=0;i<m;i++){
obsi = mobs[i];
phi = 0.0;
for(j=cobs;j<cobs+obsi;j++){
phi = phi + ((z[j] - xb[j])/v[i]); // form phi
}
awi = 0.0;
aw2i = 0.0;
for(j=0;j<m;j++){ // form awi and aw2i
aw2i = aw2i + W2[i][j]*a[j]; // (W2[i][i] = 0)
if(j != i){
awi = awi + (W[i + j*m] + W[j + i*m])*a[j];
}
}
bi = phi + (rho/sige)*awi - ((rho*rho)/sige)*aw2i;
di = (1/sige) + ((rho*rho)/sige)*w1[i] + (obsi/v[i]);
a[i] = (bi/di) + sqrt(1/di)*snorm(); // Form a[i]
cobs = cobs + obsi;
}
} // End update of a
// Compute tvec = del*a
cobs = 0;
for(i=0; i<m; i++){
obsi = mobs[i];
for(j=cobs; j<cobs + obsi; j++){
tvec[j] = a[i];
}
cobs = cobs + obsi;
}
//UPDATE: sige
matvec(Bp,m,m,a,Bpa); //form Bp*a
aBBa = 0.0; //form aBBa = a'*Bp'*Bp*a
for(i=0; i<m; i++){
aBBa = aBBa + Bpa[i]*Bpa[i];
}
chi = genchi(m + 2.0*nu);
sige = (aBBa + 2.0*d0)/chi;
//UPDATE: v (and vv = del*v)
for(i=0; i<n; i++){
e[i] = e0[i] - tvec[i]; //form e = z - x*bhat - tvec
}
cobs = 0;
for(i=0; i<m; i++){
obsi = mobs[i];
chi = genchi(rval + obsi);
ee = 0.0;
for(j=cobs; j<cobs + obsi; j++){ // form ee
ee = ee + e[j]*e[j];
}
v[i] = (ee + rval)/chi; // form v
for(j=cobs; j<cobs + obsi; j++){
vv[j] = v[i]; // form vv
}
cobs = cobs + obsi;
}
//UPDATE: rval (if necessary)
if (mm != 0.0){
rval = gengam(mm,kk);
}
//UPDATE: rho (using univariate integration)
matvec(Wmat,m,m,a,Wa); // form Wa vector
c0 = 0.0; // form a'*a
c1 = 0.0; // form a'*Wa
c2 = 0.0; // form (Wa)'*Wa
for(i=0; i<m; i++){
c0 = c0 + a[i]*a[i];
c1 = c1 + a[i]*Wa[i];
c2 = c2 + Wa[i]*Wa[i];
}
rho = draw_rho(rdet,ldet,c0,c1,c2,sige,ngrid,m,k,rho);
// (4) Update Bp matrix using new rho
for(i=0; i<m; i++){
for(j=0; j<m; j++){
if (j == i){
Bp[i][j] = 1 - rho*W[i + j*m];
}else{
Bp[i][j] = - rho*W[i + j*m];
}
}
}
//UPDATE: z
if (zflag == 1){ // skip this for continuous y-values
// (1) Generate vector of means
cobs = 0;
for(i=0; i<m; i++){
obsi = mobs[i];
for(j=cobs; j<cobs + obsi; j++){
mn[j] = xb[j] + a[i];
}
cobs = cobs + obsi;
}
// (2) Sample truncated normal for latent z values
normal_truncc(n,mn,vv,limit1,limit2,z);
// (3) Compute associated sample y vector: ys
for(i=0; i<n; i++){
if(z[i]<0){
ys[i] = 0.0;
}else{
ys[i] = 1.0;
}
}
} // end of if zflag == 1
// ===================
// Save sample draws
// ===================
if (iter > nomit-1){
pdraw[iter - nomit] = rho; //save rho-draws
sdraw[iter - nomit] = sige; //save sige-draws
if(mm != 0.0)
rdraw[iter - nomit] = rval; //save r-draws (if necessary)
for(j=0; j<k; j++)
bdraw[(iter - nomit) + j*nsave] = bhat[j]; //save beta-draws
for(i=0; i<m; i++){
vmean[i] = vmean[i] + v[i]/((double) (nsave)); //save mean v-draws
adraw[(iter - nomit) + i*nsave] = a[i]; //save a-draws
amean[i] = amean[i] + a[i]/((double) (nsave)); //save mean a-draws
}
for(i=0; i<n; i++){
zmean[i] = zmean[i] + z[i]/((double) (nsave)); //save mean z-draws
yhat[i] = yhat[i] + mn[i]/((double) (nsave)); //save mean y-values
}
}
} // End iteration loop
// ===============================
// END SAMPLER
// ===============================
// Free up allocated vectors
free_dmatrix(xs,0,n-1,0);
free_dmatrix(xst,0,k-1,0);
free_dmatrix(xsxs,0,k-1,0);
free_dmatrix(A0,0,k-1,0);
free_dmatrix(A1,0,m-1,0);
free_dmatrix(Bp,0,m-1,0);
free_dmatrix(Bpt,0,m-1,0);
free_dmatrix(W2,0,m-1,0);
free_dmatrix(xmat,0,n-1,0);
free_dmatrix(Wmat,0,m-1,0);
free_dvector(z,0);
free_dvector(tvec,0);
free_dvector(e,0);
free_dvector(e0,0);
free_dvector(xb,0);
free_dvector(mn,0);
free_dvector(ys,0);
free_dvector(limit1,0);
free_dvector(limit2,0);
free_dvector(zmt,0);
free_dvector(vv,0);
free_dvector(bhat,0);
free_dvector(b0,0);
free_dvector(b0tmp,0);
free_dvector(v,0);
free_dvector(b1,0);
free_dvector(b1tmp,0);
free_dvector(Bpa,0);
free_dvector(rdet,0);
free_dvector(ldet,0);
free_dvector(w1,0);
free_dvector(Wa,0);
} // End of semip_gc
void lsqdecomp(real_t **a, int n, int m, int n1, real_t aux[],
real_t aid[], int ci[])
{
real_t *allocate_real_vector(int, int);
void free_real_vector(real_t *, int);
real_t matmat(int, int, int, int, real_t **, real_t **);
real_t tammat(int, int, int, int, real_t **, real_t **);
void elmcol(int, int, int, int, real_t **, real_t **, real_t);
void ichcol(int, int, int, int, real_t **);
int j,k,kpiv,nr,s,fsum;
real_t beta,sigma,norm,aidk,akk,w,eps,temp,*sum;
sum=allocate_real_vector(1,m);
norm=0.0;
aux[3]=m;
nr=n1;
fsum=1;
for (k=1; k<=m; k++) {
if (k == n1+1) {
fsum=1;
nr=n;
}
if (fsum)
for (j=k; j<=m; j++) {
w=sum[j]=tammat(k,nr,j,j,a,a);
if (w > norm) norm=w;
}
fsum=0;
eps=aux[2]*sqrt(norm);
sigma=sum[k];
kpiv=k;
for (j=k+1; j<=m; j++)
if (sum[j] > sigma) {
sigma=sum[j];
kpiv=j;
}
if (kpiv != k) {
sum[kpiv]=sum[k];
ichcol(1,n,k,kpiv,a);
}
ci[k]=kpiv;
akk=a[k][k];
sigma=tammat(k,nr,k,k,a,a);
w=sqrt(sigma);
aidk=aid[k]=((akk < 0.0) ? w : -w);
if (w < eps) {
aux[3]=k-1;
break;
}
beta=1.0/(sigma-akk*aidk);
a[k][k]=akk-aidk;
for (j=k+1; j<=m; j++) {
elmcol(k,nr,j,k,a,a,-beta*tammat(k,nr,k,j,a,a));
temp=a[k][j];
sum[j] -= temp*temp;
}
if (k == n1)
for (j=n1+1; j<=n; j++)
for (s=1; s<=m; s++) {
nr = (s > n1) ? n1 : s-1;
w=a[j][s]-matmat(1,nr,j,s,a,a);
a[j][s] = (s > n1) ? w : w/aid[s];
}
}
free_real_vector(sum,1);
}
void efsirk(real_t *x, real_t xe, int m, real_t y[],
real_t *delta, void (*derivative)(int, real_t[], real_t *),
void (*jacobian)(int, real_t **, real_t [], real_t *),
real_t **j, int *n, real_t aeta, real_t reta, real_t hmin,
real_t hmax, int linear,
void (*output)(real_t, real_t, int, real_t [],
real_t, real_t **, int))
{
int *allocate_integer_vector(int, int);
real_t *allocate_real_vector(int, int);
real_t **allocate_real_matrix(int, int, int, int);
void free_integer_vector(int *, int);
void free_real_vector(real_t *, int);
void free_real_matrix(real_t **, int, int, int);
real_t vecvec(int, int, int, real_t [], real_t []);
real_t matmat(int, int, int, int, real_t **, real_t **);
real_t matvec(int, int, int, real_t **, real_t []);
void gsselm(real_t **, int, real_t [], int [], int []);
void solelm(real_t **, int, int [], int [], real_t []);
int k,l,lin,*ri,*ci;
real_t step,h,mu0,mu1,mu2,theta0,theta1,nu1,nu2,nu3,yk,fk,c1,c2,
d,*f,*k0,*labda,**j1,aux[8],discr,eta,s,z1,z2,e,alpha1,a,b;
ri=allocate_integer_vector(1,m);
ci=allocate_integer_vector(1,m);
f=allocate_real_vector(1,m);
k0=allocate_real_vector(1,m);
labda=allocate_real_vector(1,m);
j1=allocate_real_matrix(1,m,1,m);
aux[2]=FLT_EPSILON;
aux[4]=8.0;
for (k=1; k<=m; k++) f[k]=y[k];
*n = 0;
(*output)(*x,xe,m,y,*delta,j,*n);
step=0.0;
do {
(*n)++;
/* difference scheme */
(*derivative)(m,f,delta);
/* step size */
if (linear)
s=h=hmax;
else
if (*n == 1 || hmin == hmax)
s=h=hmin;
else {
eta=aeta+reta*sqrt(vecvec(1,m,0,y,y));
c1=nu3*step;
for (k=1; k<=m; k++) labda[k] += c1*f[k]-y[k];
discr=sqrt(vecvec(1,m,0,labda,labda));
s=h=(eta/(0.75*(eta+discr))+0.33)*h;
if (h < hmin)
s=h=hmin;
else
if (h > hmax) s=h=hmax;
}
if ((*x)+s > xe) s=xe-(*x);
lin=((step == s) && linear);
step=s;
if (!linear || *n == 1) (*jacobian)(m,j,y,delta);
if (!lin) {
/* coefficient */
z1=step*(*delta);
if (*n == 1) z2=z1+z1;
if (fabs(z2-z1) > 1.0e-6*fabs(z1) || z2 > -1.0) {
a=z1*z1+12.0;
b=6.0*z1;
if (fabs(z1) < 0.1)
alpha1=(z1*z1/140.0-1.0)*z1/30.0;
else if (z1 < 1.0e-14)
alpha1=1.0/3.0;
else if (z1 < -33.0)
alpha1=(a+b)/(3.0*z1*(2.0+z1));
else {
e=((z1 < 230.0) ? exp(z1) : FLT_MAX);
alpha1=((a-b)*e-a-b)/(((2.0-z1)*e-2.0-z1)*3.0*z1);
}
mu2=(1.0/3.0+alpha1)*0.25;
mu1 = -(1.0+alpha1)*0.5;
mu0=(6.0*mu1+2.0)/9.0;
theta0=0.25;
theta1=0.75;
a=3.0*alpha1;
nu3=(1.0+a)/(5.0-a)*0.5;
a=nu3+nu3;
nu1=0.5-a;
nu2=(1.0+a)*0.75;
z2=z1;
}
c1=step*mu1;
d=step*step*mu2;
for (k=1; k<=m; k++) {
for (l=1; l<=m; l++)
j1[k][l]=d*matmat(1,m,k,l,j,j)+c1*j[k][l];
j1[k][k] += 1.0;
}
gsselm(j1,m,aux,ri,ci);
}
c1=step*step*mu0;
d=step*2.0/3.0;
for (k=1; k<=m; k++) {
k0[k]=fk=f[k];
labda[k]=d*fk+c1*matvec(1,m,k,j,f);
}
solelm(j1,m,ri,ci,labda);
for (k=1; k<=m; k++) f[k]=y[k]+labda[k];
(*derivative)(m,f,delta);
c1=theta0*step;
c2=theta1*step;
d=nu1*step;
for (k=1; k<=m; k++) {
yk=y[k];
fk=f[k];
labda[k]=yk+d*fk+nu2*labda[k];
y[k]=f[k]=yk+c1*k0[k]+c2*fk;
}
(*x) += step;
(*output)(*x,xe,m,y,*delta,j,*n);
} while (*x < xe);
free_integer_vector(ri,1);
free_integer_vector(ci,1);
free_real_vector(f,1);
free_real_vector(k0,1);
free_real_vector(labda,1);
free_real_matrix(j1,1,m,1);
}
double qmTriangleAngles (MTriangle *e) {
double a = 500;
double worst_quality = std::numeric_limits<double>::max();
double mat[3][3];
double mat2[3][3];
double den = atan(a*(M_PI/9)) + atan(a*(M_PI/9));
// This matrix is used to "rotate" the triangle to get each vertex
// as the "origin" of the mapping in turn
double rot[3][3];
rot[0][0]=-1; rot[0][1]=1; rot[0][2]=0;
rot[1][0]=-1; rot[1][1]=0; rot[1][2]=0;
rot[2][0]= 0; rot[2][1]=0; rot[2][2]=1;
double tmp[3][3];
//double minAngle = 120.0;
for (int i = 0; i < e->getNumPrimaryVertices(); i++) {
const double u = i == 1 ? 1 : 0;
const double v = i == 2 ? 1 : 0;
const double w = 0;
e->getJacobian(u, v, w, mat);
e->getPrimaryJacobian(u,v,w,mat2);
for (int j = 0; j < i; j++) {
matmat(rot,mat,tmp);
memcpy(mat, tmp, sizeof(mat));
}
//get angle
double v1[3] = {mat[0][0], mat[0][1], mat[0][2] };
double v2[3] = {mat[1][0], mat[1][1], mat[1][2] };
double v3[3] = {mat2[0][0], mat2[0][1], mat2[0][2] };
double v4[3] = {mat2[1][0], mat2[1][1], mat2[1][2] };
norme(v1);
norme(v2);
norme(v3);
norme(v4);
double v12[3], v34[3];
prodve(v1,v2,v12);
prodve(v3,v4,v34);
norme(v12);
norme(v34);
double orientation;
prosca(v12,v34,&orientation);
// If the triangle is "flipped" it's no good
if (orientation < 0)
return -std::numeric_limits<double>::max();
double c;
prosca(v1,v2,&c);
double x = acos(c)-M_PI/3;
//double angle = (x+M_PI/3)/M_PI*180;
double quality = (atan(a*(x+M_PI/9)) + atan(a*(M_PI/9-x)))/den;
worst_quality = std::min(worst_quality, quality);
//minAngle = std::min(angle, minAngle);
//printf("Angle %g ", angle);
// printf("Quality %g\n",quality);
}
//printf("MinAngle %g \n", minAngle);
//return minAngle;
return worst_quality;
}
double qmTet(const double &x1, const double &y1, const double &z1,
const double &x2, const double &y2, const double &z2,
const double &x3, const double &y3, const double &z3,
const double &x4, const double &y4, const double &z4,
const qualityMeasure4Tet &cr, double *volume)
{
switch(cr){
case QMTET_ONE:
return 1.0;
case QMTET_3:
{
double mat[3][3];
mat[0][0] = x2 - x1;
mat[0][1] = x3 - x1;
mat[0][2] = x4 - x1;
mat[1][0] = y2 - y1;
mat[1][1] = y3 - y1;
mat[1][2] = y4 - y1;
mat[2][0] = z2 - z1;
mat[2][1] = z3 - z1;
mat[2][2] = z4 - z1;
*volume = fabs(det3x3(mat)) / 6.;
double l = ((x2 - x1) * (x2 - x1) +
(y2 - y1) * (y2 - y1) +
(z2 - z1) * (z2 - z1));
l += ((x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1) + (z3 - z1) * (z3 - z1));
l += ((x4 - x1) * (x4 - x1) + (y4 - y1) * (y4 - y1) + (z4 - z1) * (z4 - z1));
l += ((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2) + (z3 - z2) * (z3 - z2));
l += ((x4 - x2) * (x4 - x2) + (y4 - y2) * (y4 - y2) + (z4 - z2) * (z4 - z2));
l += ((x3 - x4) * (x3 - x4) + (y3 - y4) * (y3 - y4) + (z3 - z4) * (z3 - z4));
return 12. * pow(3 * fabs(*volume), 2. / 3.) / l;
}
case QMTET_2:
{
double mat[3][3];
mat[0][0] = x2 - x1;
mat[0][1] = x3 - x1;
mat[0][2] = x4 - x1;
mat[1][0] = y2 - y1;
mat[1][1] = y3 - y1;
mat[1][2] = y4 - y1;
mat[2][0] = z2 - z1;
mat[2][1] = z3 - z1;
mat[2][2] = z4 - z1;
*volume = fabs(det3x3(mat)) / 6.;
double p0[3] = {x1, y1, z1};
double p1[3] = {x2, y2, z2};
double p2[3] = {x3, y3, z3};
double p3[3] = {x4, y4, z4};
double s1 = fabs(triangle_area(p0, p1, p2));
double s2 = fabs(triangle_area(p0, p2, p3));
double s3 = fabs(triangle_area(p0, p1, p3));
double s4 = fabs(triangle_area(p1, p2, p3));
double rhoin = 3. * fabs(*volume) / (s1 + s2 + s3 + s4);
double l = sqrt((x2 - x1) * (x2 - x1) +
(y2 - y1) * (y2 - y1) +
(z2 - z1) * (z2 - z1));
l = std::max(l, sqrt((x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1) +
(z3 - z1) * (z3 - z1)));
l = std::max(l, sqrt((x4 - x1) * (x4 - x1) + (y4 - y1) * (y4 - y1) +
(z4 - z1) * (z4 - z1)));
l = std::max(l, sqrt((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2) +
(z3 - z2) * (z3 - z2)));
l = std::max(l, sqrt((x4 - x2) * (x4 - x2) + (y4 - y2) * (y4 - y2) +
(z4 - z2) * (z4 - z2)));
l = std::max(l, sqrt((x3 - x4) * (x3 - x4) + (y3 - y4) * (y3 - y4) +
(z3 - z4) * (z3 - z4)));
return 2. * sqrt(6.) * rhoin / l;
}
break;
case QMTET_COND:
{
/// condition number is defined as (see Knupp & Freitag in IJNME)
double INVW[3][3] = {{1,-1./sqrt(3.),-1./sqrt(6.)},{0,2/sqrt(3.),-1./sqrt(6.)},{0,0,sqrt(1.5)}};
double A[3][3] = {{x2-x1,y2-y1,z2-z1},{x3-x1,y3-y1,z3-z1},{x4-x1,y4-y1,z4-z1}};
double S[3][3],INVS[3][3];
matmat(A,INVW,S);
*volume = inv3x3(S,INVS) * 0.70710678118654762;//2/sqrt(2);
double normS = norm2 (S);
double normINVS = norm2 (INVS);
return normS * normINVS;
}
default:
Msg::Error("Unknown quality measure");
return 0.;
}
}
