Sunday, December 26, 2010

Plate Reconstruction Interpolation - Appendix II - C Code

Appendix II: C Source Code Fragments (Plate Reconstruction Interpolation):
(Based on Pilger, R. H., Jr., 2003, Geokinematics: Prelude to Geodynamics, Springer-Verlag, Berlin.)
// Code follows jump


//Coordinate Transformations:
// subroutine cdtrnx:
// convert latitude and longitude of a point to cartesian coordinates
void cdtrnx(double xlt, double xln, double *xx, double *yy, double *zz)
// xlt: latitude; xln: longitude; both in degrees
{
extern double rad;
// rad=3.14159265/180; conversion from degrees to radians; externally defined.
double cosxln,sinxln,cosxlt,sinxlt;
// xx, yy, zz: pointers corresponding to the x, y, z Cartesian coordinates
sinxlt=(*zz)=sin((*xx)=xlt*rad);
cosxlt=cos(*xx);
sinxln=sin((*yy)=xln*rad);
cosxln=cos(*yy);
(*xx)=cosxlt*cosxln;
(*yy)=cosxlt*sinxln;
return;
}
// apply rotation magnitude to unit vector (Cartesian coordinates) to produce pseudovector
wx = ww*xx;
wy = ww*yy;
wz = ww*zz;
// subroutine invcdtrn3, calling asintest, atan2t, longnorm, rotnorm, xnorm:
// convert pseudovector to spherical coordinates:
// latitude, longitude, rotation angle (positive counterclockwise)
void invcdtrn3(double xf, double yf, double zf, double *rlt, double *rln, double *rlm)
{
extern double deg;
// deg=180/3.14159265; conversion from radians to degrees; externally defined.
int i;
double drlmi,prln,prln1,prlt,rlmi,rlmt,rltt;
*rln=longnorm(atan2t(yf,xf));
rlmi=divide(1.0,*rlm=xnorm(xf,yf,zf));
*rlt=zf*rlmi;
*rlt=deg*asin(asintest(*rlt));
*rlm=rotnorm(deg*(*rlm));
return;
}
// Utility subroutines:
//function xnorm, returns vector norm:
double xnorm(double xf,double yf,double zf)
{
return(sqrtt(xf*xf+yf*yf+zf*zf));
}
// function sqrtt:
// returns square root of argument if greater than zero; 0.0 if less than or equal to zero;
double sqrtt(double temp)
{
if (temp>1.0e-40)
    return (sqrt(temp));
return(0.0);
}
// function divide:
// returns
//  inverse of argument if absolute value is greater than exp(-40);
// 0.0 otherwise;
double divide(double yy,double xx)
{
if (fabs(xx)>1.0e-40)
return(yy/xx);
return(0.0);
}
// function atan2t:
// returns
//  inverse tangent (in degrees) of dividend of numerator and denominator,
//    if denominator is greater than exp(-40) and numerator is greater than 0.0;
// 0.0 otherwise
double atan2t(double yy,double xx)
{
extern double deg;
if (fabs(xx)>1.0e-40 ||
fabs(yy)>0.0) return(deg*atan2(yy,xx));
    else
      return(0.0);
}
// function asintest:
// handles rounding error effects of absolute value greater than unity.
// returns
//  argument if absolute value is less than or equal to 1.0;
// 1.0 if greater than 1.0; -1.0 if less than –1.0;
double asintest(double x)
{
if (fabs(x)<=1.) return(x);
if (x>1.) return(1.);
return(-1.);
}
// function longnorm:
// modifies longitude argument (in degrees) to fall within limits of
// positive/negative 180 degrees.
double longnorm(double xln)
{
if (fabs(xln)>180.)
{
if (xln>0.)
return(xln-360.);
else return(xln+180.);
}
return(xln);
}
// function rotnorm:
// modifies rotation angle argument (in degrees) to fall within limits of
// positive/negative 180 degrees.
double rotnorm(double rot)
{
int i;
if (fabs(rot)>360.)
{
i=(int)(rot/360.);
rot-=i*360;
}
if (fabs(rot)>180.)
if (rot>0.0) rot-=360.;
else
rot+=360.;
return(rot);
}
// code fragment, instantaneous velocity vector (vector cross product):
vx = (wy * zz – wz * yy);
vy = (wz * xx – xx * zz);
vz = (wx * yy – wy * xx);
 // Finite Rotations:
// Computation of quaternion parameters w, x, y, z. 
// Based on derivation of Francheteau (1970)
void cdtrnp(double plt,double pln,double plm,
double *w,double *x,double*y,double *z)
// plt, pln, plm: input latitude and longitude of pole, rotation angle (degrees)
// w, x, y, z: pointers to quaternion parameter values
{
extern double rad;
// rad: as defined above in cdtrnx comment
double cpltr,plnr,sin_plnr,cos_plnr,sin_cpltr,cos_cpltr,plmr,plmr2,sin_plmr2,cos_plmr2;
if (plm<0.)
{
plm=-plm; plt=-plt; pln=pln+180;
plmr=plm*rad;
cos_plmr2=cos(plmr2=plmr*0.5);
sin_plmr2=sin(plmr2);
cos_cpltr=cos(cpltr=(90.-plt)*rad);
sin_cpltr=sin(cpltr);
sin_plnr=sin(plnr=pln*rad);
cos_plnr=cos(plnr);
if (plm>0.)
{
*w=cos_plmr2;
*x=sin_plmr2*sin_cpltr;
*y=(*x)*sin_plnr;
(*x)*=cos_plnr;
*z=sin_plmr2*cos_cpltr;
            }
else
{
*w=1; *y=0.; *x=0.; *z=0.;
}
return;
}
 // function rotmult2
// called by rotprod
double rotmult2(double w,double x,double y,double z)
{
return(w*x+y*z);
}
// function rotmult4
// called by rotprod
double rotmult4(double w1,double w2,double x1,double x2,
double y1,double y2,double z1,double z2)
{
return(w1*w2+x1*x2+y1*y2+z1*z2);
}
// subroutine rotprod:
// calls functions rotmult4 and rotmult2, below
// based on derivation of Francheteau (1970)
void rotprod(double w,double x,double y,double z,
double xx,double yy,double zz,
double *rx,double *ry,double *rz)
// w, x, y, z: rotation parameters calculated from cdtrnp
// xx, yy, zz: unit vector calculated from cdtrnx
// rx, ry, rz: pointers to rotated unit vector values.
{
*rx=rotmult4(w,w,x,x,-y,y,-z,z)*xx + 2.*rotmult2(-w,z,x,y)*yy + 2.*rotmult2(w,y,x,z)*zz;
*ry=2.*rotmult2(w,z,x,y)*xx+rotmult4(w,w,-x,x,y,y,-z,z)*yy + 2.*rotmult2(-w,x,y,z)*zz;
*rz=2.*rotmult2(-w,y,x,z)*xx+2.*rotmult2(w,x,y,z)*yy + rotmult4(w,w,-x,x,-y,y,z,z)*zz;
}
// addmult: called by combrots, below
double addmult(double w1,double x1,double y1,double z1,
double w2,double x2,double y2,double z2)
{
return(w1*w2+x1*x2+y1*y2+z1*z2);
}
/* Combine rotation parameters (w1-x1-y1-z1; w2-x2-y2-z2) for two rotations to produce parameters (*wt-*xt-*yt-*zt) of total rotation; calls addmult. Based on derivation of Francheteau (1970) */
void combrots(double w1,double x1,double y1,double z1,double w2,double x2,
double y2,double z2,double*wt,double *xt,double *yt, double *zt)
// w1, x1, y1, z1: parametersfor first rotation
// w2, x2, y2, z2: parameters for second rotation
// wt, xt, yt, zt: pointers to parameter values for total rotation
{
*w=addmult(w1,-x1,-y1,-z1,w2,x2,y2,z2);
*x=addmult(w1,x1,-y1,z1,x2,w2,z2,y2);
*y=addmult(w1,x1,y1,-z1,y2,z2,w2,x2);
*z=addmult(w1,-x1,y1,z1,z2,y2,x2,w2);
if ((*w)<0.)
{
w=-(*w); *x=-(*x); *y=-(*y);
(*z)=-(*z);
}
return;
}
/* calculate spherical coordinates of rotation from quaternion parameters; calls asintest, longnorm, rotnorm (above) */
void invrotpars(double w, double x, double y, double z,double *plt,double *pln,double *plm)
// w, x, y, z: quaternion rotation parameters
// plt, pln, plm: pointers to spherical rotation parameters
{
// deg: as defined in invcdtrn3 abovedouble spltr,cplt,cpltr;

extern deg;

spltr=sin(*plm=acos(asintest(w)));
cplt=deg*acos(cpltr=asintest(divide(z,spltr)));
*pln=longnorm(atan2t(y,x));
*plm=rotnorm((*plm)*2.*deg);
*plt=90.-cplt;
if (fabs(*plt)>90.)
{
if (*plt>0.0)
{
*plt=180-(*plt);
*pln=(*pln)+180.;
}
else
{
*plt=-180-(*plt);
*pln=(*pln)+180.;
}
}
return;
}
/*Instantaneous pole calculaton from cubic spline interpolation of rotation parameters and first derivative of rotation parameters. Method of Smith (1981) modified by Pilger (2003) */
// subroutine invpcoor3
// given ww[], vector equivalent of rotation pole, magnitude = average rotation rate
// and first time derivative of the vector, pw[],
// compute instantaneous pseudovector, *xx, *yy, *zz,
// and spherical coordinate equivalent, *rlt, *rln, *rlm
void invpcoor3(double age,double *ww,double *pw,double *xx, double *yy, double *zz, double *rlt, double *rln, double *rlm)
{
extern double deg,rad; // deg, rad: as defined above
double ang,cang,cang1,pang,pxi,pxx,pyy,pzz,  rlmi,rlmi2,rlmt,sang,wmagi,www,wx,wy,wz,pww,pwx,pwy,pwz,
wxpywypx,wypzwzpy,wzpxwxpz;
www=xnorm(ww[0],ww[1],ww[2]) // *ww is vector ww[]
wmagi=divide(1.,www);
wx=ww[0]*wmagi;
wy=ww[1]*wmagi; wz=ww[2]*wmagi;
sang=sin(ang=www*age);
cang=cos(ang);
cang1=cang-1;
pww=wx*pw[0]+wy*pw[1]+wz*pw[2]; // *pw is vector pw[], time derivative of ww[]pang=www+pww*age;
pwx=(pw[0]-wx*pww)*wmagi;
pwy=(pw[1]-wy*pww)*wmagi;
pwz=(pw[2]-wz*pww)*wmagi;
// vector cross product:
wypzwzpy=wy*pwz-wz*pwy;
wzpxwxpz=wz*pwx-wx*pwz;
wxpywypx=wx*pwy-wy*pwx;
*xx=pang*wx+sang*pwx+cang1*wypzwzpy;
*yy=pang*wy+sang*pwy+cang1*wzpxwxpz;
*zz=pang*wz+sang*pwz+cang1*wxpywypx;
invcdtrn3(*xx,*yy,*zz,rlt,rln,rlm);
}

© 2010 Rex H. Pilger, Jr.

No comments:

Post a Comment