/**
* Taken the path from A to B over a sphere return the bearing (0..2PI) at the destination point B.
*/
double GetSphericalBearingFinal(double aLon, double aLat,
double bLon, double bLat)
{
aLon=aLon*M_PI/180;
aLat=aLat*M_PI/180;
bLon=bLon*M_PI/180;
bLat=bLat*M_PI/180;
double dLon=aLon-bLon;
double sindLon, sinaLat, sinbLat;
double cosdLon, cosaLat, cosbLat;
sincos(dLon, sindLon, cosdLon);
sincos(aLat, sinaLat, cosaLat);
sincos(bLat, sinbLat, cosbLat);
double y=sindLon*cosaLat;
double x=cosbLat*sinaLat-sinbLat*cosaLat*cosdLon;
double bearing=atan2(y,x);
if (bearing>=0) {
bearing-=M_PI;
}
else {
bearing+=M_PI;
}
//double bearing=fmod(atan2(y,x)+3*M_PI,2*M_PI);
return bearing;
}
int
p_sincos(value val_arg, type tag_arg,
value val_sin, type tag_sin,
value val_cos, type tag_cos)
{
extern void sincos(); /* from the math library */
double s, c;
Prepare_Requests;
Error_If_Ref(tag_arg);
Check_Output_Float(tag_sin);
Check_Output_Float(tag_cos);
if (IsDouble(tag_arg))
sincos(Dbl(val_arg), &s, &c);
else if (IsInteger(tag_arg))
sincos((double) val_arg.nint, &s, &c);
else
{
Error(TYPE_ERROR);
}
Request_Unify_Float(val_sin, tag_sin, s);
Request_Unify_Float(val_cos, tag_cos, c);
Return_Unify;
}
static LP e_inverse (XY xy, PJ *P) { /* Ellipsoidal, inverse */
LP lp = {0.0,0.0};
struct pj_opaque *Q = P->opaque;
double sin_Cn, cos_Cn, cos_Ce, sin_Ce, dCn, dCe;
double Cn = xy.y, Ce = xy.x;
/* normalize N, E */
Cn = (Cn - Q->Zb)/Q->Qn;
Ce = Ce/Q->Qn;
if (fabs(Ce) <= 2.623395162778) { /* 150 degrees */
/* norm. N, E -> compl. sph. LAT, LNG */
Cn += clenS(Q->utg, PROJ_ETMERC_ORDER, 2*Cn, 2*Ce, &dCn, &dCe);
Ce += dCe;
Ce = atan (sinh (Ce)); /* Replaces: Ce = 2*(atan(exp(Ce)) - FORTPI); */
/* compl. sph. LAT -> Gaussian LAT, LNG */
#ifdef _GNU_SOURCE
sincos (Cn, &sin_Cn, &cos_Cn);
sincos (Ce, &sin_Ce, &cos_Ce);
#else
sin_Cn = sin (Cn);
cos_Cn = cos (Cn);
sin_Ce = sin (Ce);
cos_Ce = cos (Ce);
#endif
Ce = atan2 (sin_Ce, cos_Ce*cos_Cn);
Cn = atan2 (sin_Cn*cos_Ce, hypot (sin_Ce, cos_Ce*cos_Cn));
/* Gaussian LAT, LNG -> ell. LAT, LNG */
lp.phi = gatg (Q->cgb, PROJ_ETMERC_ORDER, Cn);
lp.lam = Ce;
}
else
lp.phi = lp.lam = HUGE_VAL;
return lp;
}
// This might be a convenient function to use in fgcalc, rather than repeating the same code six times
int make_proj_mat(float phi, float theta, float psi, float * dm)
{
// float cphi=cos(phi);
// float sphi=sin(phi);
// float cthe=cos(theta);
// float sthe=sin(theta);
// float cpsi=cos(psi);
// float spsi=sin(psi);
double cphi, sphi, cthe, sthe, cpsi, spsi;
double dphi = phi;
double dthe = theta;
double dpsi = psi;
sincos(dphi, &sphi, &cphi);
sincos(dthe, &sthe, &cthe);
sincos(dpsi, &spsi, &cpsi);
dm[0]=cphi*cthe*cpsi-sphi*spsi;
dm[1]=sphi*cthe*cpsi+cphi*spsi;
dm[2]=-sthe*cpsi;
dm[3]=-cphi*cthe*spsi-sphi*cpsi;
dm[4]=-sphi*cthe*spsi+cphi*cpsi;
dm[5]=sthe*spsi;
return 0;
}
int small_circle_lambda_by_phi(double phi0, double lambda0, double d, double phi, double * p) {
double sin_phi0, cos_phi0;
double sin_phi, cos_phi;
double cos_mult, cos_div;
sincos(phi0, &sin_phi0, &cos_phi0);
sincos(phi, &sin_phi, &cos_phi);
cos_mult = cos_phi0 * cos_phi;
if (cos_mult == 0.0) {
return 0;
}
cos_div = (cos(d) - sin_phi0 * sin_phi) / cos_mult;
if (cos_div == -1.0 || cos_div == 1.0) {
*p = 0.0;
return 1;
}
if (cos_div < -1.0 || cos_div > 1.0) {
return 0;
}
*p = acos(cos_div);
return 1;
}
static XY e_forward (LP lp, PJ *P) { /* Ellipsoidal, forward */
XY xy = {0.0,0.0};
struct pj_opaque *Q = P->opaque;
double sin_Cn, cos_Cn, cos_Ce, sin_Ce, dCn, dCe;
double Cn = lp.phi, Ce = lp.lam;
/* ell. LAT, LNG -> Gaussian LAT, LNG */
Cn = gatg (Q->cbg, PROJ_ETMERC_ORDER, Cn);
/* Gaussian LAT, LNG -> compl. sph. LAT */
#ifdef _GNU_SOURCE
sincos (Cn, &sin_Cn, &cos_Cn);
sincos (Ce, &sin_Ce, &cos_Ce);
#else
sin_Cn = sin (Cn);
cos_Cn = cos (Cn);
sin_Ce = sin (Ce);
cos_Ce = cos (Ce);
#endif
Cn = atan2 (sin_Cn, cos_Ce*cos_Cn);
Ce = atan2 (sin_Ce*cos_Cn, hypot (sin_Cn, cos_Cn*cos_Ce));
/* compl. sph. N, E -> ell. norm. N, E */
Ce = asinhy ( tan (Ce) ); /* Replaces: Ce = log(tan(FORTPI + Ce*0.5)); */
Cn += clenS (Q->gtu, PROJ_ETMERC_ORDER, 2*Cn, 2*Ce, &dCn, &dCe);
Ce += dCe;
if (fabs (Ce) <= 2.623395162778) {
xy.y = Q->Qn * Cn + Q->Zb; /* Northing */
xy.x = Q->Qn * Ce; /* Easting */
} else
xy.x = xy.y = HUGE_VAL;
return xy;
}
int small_circle_phi_by_lambda(
double phi0, double lambda0, double d, double lambda, double * phi1, double * phi2
) {
double sin_phi0, cos_phi0;
double d_lambda, sin_d_lambda, cos_d_lambda;
double cos_mult, denom, phi, gamma, cos_div;
d_lambda = lambda - lambda0;
sincos(phi0, &sin_phi0, &cos_phi0);
sincos(d_lambda, &sin_d_lambda, &cos_d_lambda);
cos_mult = cos_phi0 * cos_d_lambda;
denom = sqrt(sin_phi0 * sin_phi0 + cos_mult * cos_mult);
if (denom == 0.0) {
return 0;
}
cos_div = cos(d) / denom;
if (cos_div < -1.0 || cos_div > 1.0) {
return 0;
}
phi = asin(cos_div);
gamma = acos(sin_phi0 / denom);
*phi1 = phi - gamma;
if (phi == M_PI_2) {
return 0;
}
*phi2 = M_PI - phi - gamma;
return 1;
}
void horizontal_to_equatorial(double az_deg, double el_deg, time_t lst_s, double lat_deg,
double* ra_hours, double* dec_deg)
{
double ha = 0.0;
double dec = 0.0;
double sa, ca, se, ce, sp, cp, x, y, z, r;
sincos(from_degrees(az_deg), &sa, &ca);
sincos(from_degrees(el_deg), &se, &ce);
sincos(from_degrees(lat_deg), &sp, &cp);
/* HA,Dec as x,y,z */
x = -ca * ce * sp + se * cp;
y = -sa * ce;
z = ca * ce * cp + se * sp;
/* To spherical */
r = sqrt(x * x + y * y);
ha = (r == 0.0) ? 0.0 : atan2(y, x);
dec = atan2(z, r);
*ra_hours = to_hours(from_seconds(lst_s) - ha);
*ra_hours = wrap_to(*ra_hours, 24.0);
*dec_deg = to_degrees(dec);
}
void
norm(struct place *gg, struct place *pp, struct coord *tw)
{
register struct place *g; /*geographic coords */
register struct place *p; /* new pole in old coords*/
struct place m; /* standard map coords*/
g = gg;
p = pp;
if(p->nlat.s == 1.) {
if(p->wlon.l+tw->l == 0.)
return;
g->wlon.l -= p->wlon.l+tw->l;
} else {
if(p->wlon.l != 0) {
g->wlon.l -= p->wlon.l;
sincos(&g->wlon);
}
m.nlat.s = p->nlat.s * g->nlat.s
+ p->nlat.c * g->nlat.c * g->wlon.c;
m.nlat.c = sqrt(1. - m.nlat.s * m.nlat.s);
m.nlat.l = atan2(m.nlat.s, m.nlat.c);
m.wlon.s = g->nlat.c * g->wlon.s;
m.wlon.c = p->nlat.c * g->nlat.s
- p->nlat.s * g->nlat.c * g->wlon.c;
m.wlon.l = atan2(m.wlon.s, - m.wlon.c)
- tw->l;
*g = m;
}
sincos(&g->wlon);
if(g->wlon.l>PI)
g->wlon.l -= 2*PI;
else if(g->wlon.l<-PI)
g->wlon.l += 2*PI;
}
static int
azimuth(struct place *place)
{
if(place->nlat.c < FUZZ) {
az.l = PI/2 + place->nlat.l - place->wlon.l;
sincos(&az);
rad.l = fabs(place->nlat.l - p0.l);
if(rad.l > PI)
rad.l = 2*PI - rad.l;
sincos(&rad);
return 1;
}
rad.c = trigclamp(p0.s*place->nlat.s + /* law of cosines */
p0.c*place->nlat.c*place->wlon.c);
rad.s = sqrt(1 - rad.c*rad.c);
if(fabs(rad.s) < .001) {
az.s = 0;
az.c = 1;
} else {
az.s = trigclamp(p0.c*place->wlon.s/rad.s); /* sines */
az.c = trigclamp((p0.s - rad.c*place->nlat.s)
/(rad.s*place->nlat.c));
}
rad.l = atan2(rad.s, rad.c);
return 1;
}
/**
* Taken the path from A to B over a sphere return the bearing (0..2PI) at the starting point A.
*/
double GetSphericalBearingInitial(double aLon, double aLat,
double bLon, double bLat)
{
aLon=aLon*M_PI/180;
aLat=aLat*M_PI/180;
bLon=bLon*M_PI/180;
bLat=bLat*M_PI/180;
double dLon=bLon-aLon;
double sindLon, sinaLat, sinbLat;
double cosdLon, cosaLat, cosbLat;
sincos(dLon, sindLon, cosdLon);
sincos(aLat, sinaLat, cosaLat);
sincos(bLat, sinbLat, cosbLat);
double y=sindLon*cosbLat;
double x=cosaLat*sinbLat-sinaLat*cosbLat*cosdLon;
double bearing=atan2(y,x);
//double bearing=fmod(atan2(y,x)+2*M_PI,2*M_PI);
return bearing;
}
void gwsincos12by2_weighted (
void *dd_data_arg,
unsigned long x,
unsigned long upper_x,
unsigned long N,
unsigned long col,
double *results)
{
dd_real twopi_over_N, sine, cosine, sine2, cosine2;
dd_real temp, bpower, weight, inv_weight;
x86_FIX
temp = (double) col * dd_data->gw__num_b_per_word;
bpower = gwceil (temp) - temp;
if (! dd_data->gw__c_is_one) bpower += dd_data->gw__logb_abs_c_div_fftlen * (double) col;
weight = exp (dd_data->gw__logb * bpower);
inv_weight = dd_data->gw__over_fftlen / weight;
twopi_over_N = dd_real::_2pi / (double) N;
sincos (twopi_over_N * (double) x, sine, cosine);
sine2 = sine * cosine * 2.0;
cosine2 = sqr (cosine) - sqr (sine);
sine = sine + epsilon; // Hack to avoid divide-by-zero errors
sine2 = sine2 + epsilon;
results[0] = weight;
results[2] = inv_weight;
results[4] = sine * weight;
results[6] = cosine / sine;
results[8] = sine * inv_weight;
results[10] = sine2 * weight;
results[12] = cosine2 / sine2;
results[14] = sine2 * inv_weight;
sincos (twopi_over_N * (double) upper_x, sine, cosine);
sine2 = sine * cosine * 2.0;
cosine2 = sqr (cosine) - sqr (sine);
sine = sine + epsilon; // Hack to avoid divide-by-zero errors
sine2 = sine2 + epsilon;
results[1] = weight;
results[3] = inv_weight;
results[5] = sine * weight;
results[7] = cosine / sine;
results[9] = sine * inv_weight;
results[11] = sine2 * weight;
results[13] = cosine2 / sine2;
results[15] = sine2 * inv_weight;
END_x86_FIX
}
static mat3 from_euler_angles(scalar_t x, scalar_t y, scalar_t z)
{
scalar_t cx, sx, cy, sy, cz, sz;
sincos(x, sx, cx);
sincos(y, sy, cy);
sincos(z, sz, cz);
return mat3(
vec3(cy * cz, sy * sx - cy * sz * cx, cy * sz * sx + sy * cx),
vec3(sz, cz * cx, -cz * sx),
vec3(-sy * cz, sy * sz * cx + cy * sx, cy * cx - sy * sz * sx));
}
quaternion<Real> quaternion<Real>::fromAnglesInertialToObject(Real pitch, Real bank, Real heading)
{
Real sp, sb, sh, cp, cb, ch;
sincos(pitch, &sp, &cp);
sincos(bank, &sb, &cb);
sincos(heading, &sh, &ch);
return quaternion<Real>(
cp*cb*ch + sp*sb*sh,
-ch*sp*cb - sh*cp*sb,
ch*sp*sb - sh*cp*cb,
sh*sp*cb - ch*cp*sb);
}
//--------------------------------------------------------------------
//(BODY INVERSE KINEMATICS)
//BodyRotX - Global Input pitch of the body
//BodyRotY - Global Input rotation of the body
//BodyRotZ - Global Input roll of the body
//RotationY - Input Rotation for the gait
//posX - Input position of the feet X
//posZ - Input position of the feet Z
//SinB - Sin buffer for BodyRotX
//CosB - Cos buffer for BodyRotX
//SinG - Sin buffer for BodyRotZ
//CosG - Cos buffer for BodyRotZ
//lBodyX - Output Position X of feet with Rotation
//lBodyY - Output Position Y of feet with Rotation
//lBodyZ - Output Position Z of feet with Rotation
void PhoenixCore::getBodyIK(u8 leg, s16 posX, s16 posZ, s16 posY, s16 RotationY, long *x, long *y, long *z)
{
s16 sinA4; //Sin buffer for BodyRotX calculations
s16 cosA4; //Cos buffer for BodyRotX calculations
s16 sinB4; //Sin buffer for BodyRotX calculations
s16 cosB4; //Cos buffer for BodyRotX calculations
s16 sinG4; //Sin buffer for BodyRotZ calculations
s16 cosG4; //Cos buffer for BodyRotZ calculations
s16 CPR_X; //Final X value for centerpoint of rotation
s16 CPR_Y; //Final Y value for centerpoint of rotation
s16 CPR_Z; //Final Z value for centerpoint of rotation
//Calculating totals from center of the body to the feet
CPR_X = (s16)pgm_read_word(&TBL_OFFSET_X[leg])+posX + mPtrCtrlState->c3dBodyRotOff.x;
CPR_Y = posY + mPtrCtrlState->c3dBodyRotOff.y; //Define centerpoint for rotation along the Y-axis
CPR_Z = (s16)pgm_read_word(&TBL_OFFSET_Z[leg]) + posZ + mPtrCtrlState->c3dBodyRotOff.z;
//Successive global rotation matrix:
//Math shorts for rotation: Alfa [A] = Xrotate, Beta [B] = Zrotate, Gamma [G] = Yrotate
//Sinus Alfa = SinA, cosinus Alfa = cosA. and so on...
//First calculate sinus and cosinus for each rotation:
sincos(mPtrCtrlState->c3dBodyRot.x+mTotalXBal1, &sinG4, &cosG4);
sincos(mPtrCtrlState->c3dBodyRot.z+mTotalZBal1, &sinB4, &cosB4);
if (mBoolUpsideDown)
sincos(-mPtrCtrlState->c3dBodyRot.y + (-RotationY * DEC_EXP_1) + mTotalYBal1, &sinA4, &cosA4) ;
else
sincos(mPtrCtrlState->c3dBodyRot.y + (RotationY * DEC_EXP_1) + mTotalYBal1, &sinA4, &cosA4) ;
//Calcualtion of rotation matrix:
*x = ((long)CPR_X*DEC_EXP_2 -
((long)CPR_X*DEC_EXP_2*cosA4/DEC_EXP_4*cosB4/DEC_EXP_4 -
(long)CPR_Z*DEC_EXP_2*cosB4/DEC_EXP_4*sinA4/DEC_EXP_4 +
(long)CPR_Y*DEC_EXP_2*sinB4/DEC_EXP_4)) / DEC_EXP_2;
*z = ((long)CPR_Z*DEC_EXP_2 -
( (long)CPR_X*DEC_EXP_2*cosG4/DEC_EXP_4*sinA4/DEC_EXP_4 +
(long)CPR_X*DEC_EXP_2*cosA4/DEC_EXP_4*sinB4/DEC_EXP_4*sinG4/DEC_EXP_4 +
(long)CPR_Z*DEC_EXP_2*cosA4/DEC_EXP_4*cosG4/DEC_EXP_4 -
(long)CPR_Z*DEC_EXP_2*sinA4/DEC_EXP_4*sinB4/DEC_EXP_4*sinG4/DEC_EXP_4 -
(long)CPR_Y*DEC_EXP_2*cosB4/DEC_EXP_4*sinG4/DEC_EXP_4 )) / DEC_EXP_2;
*y = ((long)CPR_Y *DEC_EXP_2 -
( (long)CPR_X*DEC_EXP_2*sinA4/DEC_EXP_4*sinG4/DEC_EXP_4 -
(long)CPR_X*DEC_EXP_2*cosA4/DEC_EXP_4*cosG4/DEC_EXP_4*sinB4/DEC_EXP_4 +
(long)CPR_Z*DEC_EXP_2*cosA4/DEC_EXP_4*sinG4/DEC_EXP_4 +
(long)CPR_Z*DEC_EXP_2*cosG4/DEC_EXP_4*sinA4/DEC_EXP_4*sinB4/DEC_EXP_4 +
(long)CPR_Y*DEC_EXP_2*cosB4/DEC_EXP_4*cosG4/DEC_EXP_4 )) / DEC_EXP_2;
}
int pmRotMatConvert(PmRotationVector const * const r, PmRotationMatrix * const m)
{
double s, c, omc;
#ifdef PM_DEBUG
if (!pmRotIsNorm(r)) {
#ifdef PM_PRINT_ERROR
pmPrintError("Bad vector in pmRotMatConvert\n");
#endif
return pmErrno = PM_NORM_ERR;
}
#endif
sincos(r->s, &s, &c);
/* from space book */
m->x.x = c + pmSq(r->x) * (omc = 1 - c); /* omc = One Minus Cos */
m->y.x = -r->z * s + r->x * r->y * omc;
m->z.x = r->y * s + r->x * r->z * omc;
m->x.y = r->z * s + r->y * r->x * omc;
m->y.y = c + pmSq(r->y) * omc;
m->z.y = -r->x * s + r->y * r->z * omc;
m->x.z = -r->y * s + r->z * r->x * omc;
m->y.z = r->x * s + r->z * r->y * omc;
m->z.z = c + pmSq(r->z) * omc;
return pmErrno = 0;
}
dgInt32 dgCollisionCone::CalculatePlaneIntersectionSimd (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 count;
if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) {
simd_128 normalYZ ((simd_128&) normal & simd_128 (0, -1, -1, 0));
normalYZ = normalYZ * normalYZ.DotProduct(normalYZ).InvSqrt();
dgVector sincos (normalYZ);
dgVector normal1 (normal.m_x, normal.m_y * sincos.m_y + normal.m_z * sincos.m_z, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector origin1 (origin.m_x, origin.m_y * sincos.m_y + origin.m_z * sincos.m_z,
origin.m_z * sincos.m_y - origin.m_y * sincos.m_z, dgFloat32 (0.0f));
count = dgCollisionConvex::CalculatePlaneIntersectionSimd (normal1, origin1, contactsOut);
for (dgInt32 i = 0; i < count; i ++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * sincos.m_y - z * normal.m_z;
contactsOut[i].m_z = z * sincos.m_y + y * normal.m_z;
}
} else {
count = dgCollisionConvex::CalculatePlaneIntersectionSimd (normal, origin, contactsOut);
}
return count;
}
inline
VectorPtr<2, T> & VectorPtr<2, T>::rotate(T angle)
{
real_t sin, cos;
sincos(angle, & sin, & cos);
return rotate(static_cast<T>(sin), static_cast<T>(cos));
};
inline
#endif
static double
clenS(double *a, int size, double arg_r, double arg_i, double *R, double *I) {
double *p, r, i, hr, hr1, hr2, hi, hi1, hi2;
double sin_arg_r, cos_arg_r, sinh_arg_i, cosh_arg_i;
/* arguments */
p = a + size;
#ifdef _GNU_SOURCE
sincos(arg_r, &sin_arg_r, &cos_arg_r);
#else
sin_arg_r = sin(arg_r);
cos_arg_r = cos(arg_r);
#endif
sinh_arg_i = sinh(arg_i);
cosh_arg_i = cosh(arg_i);
r = 2*cos_arg_r*cosh_arg_i;
i = -2*sin_arg_r*sinh_arg_i;
/* summation loop */
for (hi1 = hr1 = hi = 0, hr = *--p; a - p;) {
hr2 = hr1;
hi2 = hi1;
hr1 = hr;
hi1 = hi;
hr = -hr2 + r*hr1 - i*hi1 + *--p;
hi = -hi2 + i*hr1 + r*hi1;
}
r = sin_arg_r*cosh_arg_i;
i = cos_arg_r*sinh_arg_i;
*R = r*hr - i*hi;
*I = r*hi + i*hr;
return(*R);
}
mat4::mat4( float a, const vec3& v )
{
SLMATH_VEC_ASSERT( length(v) > FLT_MIN );
float s, c;
sincos( a, &s, &c );
const float t = 1.f - c;
const vec3 n = normalize( v );
const float x = n.x;
const float y = n.y;
const float z = n.z;
register vec4* const m = v4();
m[0][0] = t*x*x + c;
m[0][1] = t*x*y + z*s;
m[0][2] = t*x*z - y*s;
m[0][3] = 0.f;
m[1][0] = t*x*y - z*s;
m[1][1] = t*y*y + c;
m[1][2] = t*y*z + x*s;
m[1][3] = 0.f;
m[2][0] = t*x*z + y*s;
m[2][1] = t*y*z - x*s;
m[2][2] = t*z*z + c;
m[2][3] = 0.f;
m[3][0] = 0.f;
m[3][1] = 0.f;
m[3][2] = 0.f;
m[3][3] = 1.f;
}
static inline Real _y_smalln(UnsignedInteger n, Real z)
{
assert(n <= 2 && n >= 0);
if(n == 0)
{
return - std::cos(z) / z;
}
Real sin_z;
Real cos_z;
sincos(z, &sin_z, &cos_z);
const Real z_r(1. / z);
if(n == 1)
{
return - (cos_z * z_r + sin_z) * z_r;
}
else //if(n == 2)
{
const Real _3_zsq(3. * z_r * z_r);
return (1 - _3_zsq) * cos_z * z_r - _3_zsq * sin_z;
}
}
double f3(double x)
{
double s, c, tmp;
sincos (x, &tmp, &c);
s = sin (x);
return s + c;
}
double f2(double x)
{
double s, c, tmp;
sincos (x, &s, &tmp);
c = cos (x);
return s + c;
}
/* Initialize the Azimuthal projection
----------------------------------*/
long azimforint
(
double r_maj, /* major axis */
double center_lon, /* center longitude */
double center_lat, /* center latitude */
double false_east, /* x offset in meters */
double false_north /* y offset in meters */
)
{
/* Place parameters in static storage for common use
-------------------------------------------------*/
r_major = r_maj;
lon_center = center_lon;
lat_origin = center_lat;
false_northing = false_north;
false_easting = false_east;
sincos(center_lat,&sin_p12,&cos_p12);
/* Report parameters to the user
-----------------------------*/
gctp_print_title("AZIMUTHAL EQUIDISTANT");
gctp_print_radius(r_major);
gctp_print_cenlonmer(lon_center);
gctp_print_origin(lat_origin);
gctp_print_offsetp(false_easting,false_northing);
return(OK);
}
/* Orthographic forward equations--mapping lat,long to x,y
---------------------------------------------------*/
long orthfor
(
double lon, /* (I) Longitude */
double lat, /* (I) Latitude */
double *x, /* (O) X projection coordinate */
double *y /* (O) Y projection coordinate */
)
{
double sinphi, cosphi; /* sin and cos value */
double dlon; /* delta longitude value */
double coslon; /* cos of longitude */
double ksp; /* scale factor */
double g;
/* Forward equations
-----------------*/
dlon = adjust_lon(lon - lon_center);
sincos(lat,&sinphi,&cosphi);
coslon = cos(dlon);
g = sin_p14 * sinphi + cos_p14 * cosphi * coslon;
ksp = 1.0;
if ((g > 0) || (fabs(g) <= EPSLN))
{
*x = false_easting + r_major * ksp * cosphi * sin(dlon);
*y = false_northing + r_major * ksp * (cos_p14 * sinphi -
sin_p14 * cosphi * coslon);
}
else
{
GCTP_PRINT_ERROR("Point can not be projected");
return(143);
}
return(OK);
}
void
EinsplineSetLocal::evaluate (const ParticleSet& P, int iat,
ValueVector_t& psi)
{
PosType r (P.R[iat]);
PosType ru(PrimLattice.toUnit(P.R[iat]));
ru[0] -= std::floor (ru[0]);
ru[1] -= std::floor (ru[1]);
ru[2] -= std::floor (ru[2]);
for(int j=0; j<OrbitalSetSize; j++) {
complex<double> val;
Orbitals[j]->evaluate(ru, val);
double phase = -dot(r, Orbitals[j]->kVec);
double s,c;
sincos (phase, &s, &c);
complex<double> e_mikr (c,s);
val *= e_mikr;
#ifdef QMC_COMPLEX
psi[j] = val;
#else
psi[j] = real(val);
#endif
}
}
void gwsincos125by2 (
unsigned long x,
unsigned long N,
double *results)
{
dd_real arg1, sine, cosine, sine2, cosine2, sine4, cosine4, sine5, cosine5;
x86_FIX
arg1 = dd_real::_2pi * (double) x / (double) N;
sincos (arg1, sine, cosine);
results[0] = sine;
results[0] += epsilon; /* Protect against divide by zero */
results[2] = cosine / results[0];
sine2 = sine * cosine * 2.0;
cosine2 = sqr (cosine) - sqr (sine);
results[4] = sine2;
results[4] += epsilon; /* Protect against divide by zero */
results[6] = cosine2 / results[4];
sine4 = sine2 * cosine2 * 2.0;
cosine4 = sqr (cosine2) - sqr (sine2);
sine5 = sine * cosine4 + sine4 * cosine;
cosine5 = cosine * cosine4 - sine * sine4;
results[8] = sine5;
results[8] += epsilon; /* Protect against divide by zero */
results[10] = cosine5 / results[8];
END_x86_FIX
}
void gwsincos3 (
unsigned long x,
unsigned long N,
double *results)
{
dd_real arg1, sine, cosine, sine2, cosine2, sine3, cosine3;
x86_FIX
arg1 = dd_real::_2pi * (double) x / (double) N;
sincos (arg1, sine, cosine);
results[0] = sine;
results[0] += epsilon; /* Protect against divide by zero */
results[1] = cosine / results[0];
sine2 = sine * cosine * 2.0;
cosine2 = sqr (cosine) - sqr (sine);
results[2] = sine2;
results[2] += epsilon; /* Protect against divide by zero */
results[3] = cosine2 / results[2];
sine3 = sine * cosine2 + sine2 * cosine;
cosine3 = cosine * cosine2 - sine * sine2;
results[4] = sine3;
results[4] += epsilon; /* Protect against divide by zero */
results[5] = cosine3 / results[4];
END_x86_FIX
}
/**
* Calculate a perspective projection transformation.
*
* @param m the matrix to save the transformation in
* @param fovy the field of view in the y direction
* @param aspect the view aspect ratio
* @param zNear the near clipping plane
* @param zFar the far clipping plane
*/
static void
perspective (GLfloat *m, GLfloat fovy, GLfloat aspect, GLfloat zNear, GLfloat zFar)
{
GLfloat tmp[16];
double sine, cosine, cotangent, deltaZ;
GLfloat radians = fovy / 2 * M_PI / 180;
identity (tmp);
deltaZ = zFar - zNear;
sincos (radians, &sine, &cosine);
if ((deltaZ == 0) || (sine == 0) || (aspect == 0))
return;
cotangent = cosine / sine;
tmp[0] = cotangent / aspect;
tmp[5] = cotangent;
tmp[10] = -(zFar + zNear) / deltaZ;
tmp[11] = -1;
tmp[14] = -2 * zNear * zFar / deltaZ;
tmp[15] = 0;
memcpy (m, tmp, sizeof(tmp));
}
/* 1 order spectra */
static void
mylm_fit_single_pf_1d(double *p, double *x, int m, int n, void *data) {
fit_double_point_dataf *dp=(fit_double_point_dataf*)data;
int ci,cj,ck;
/* reference freq is first freq */
double ll,mm,dpow;
double sbpa;
double cbpa;
double invbmaj;
double invbmin;
double fratio;
/* actual pixels per freq */
int N=n/dp->Nf;
/* first reset x to all zeros */
memset(x,0,sizeof(double)*n);
ck=0;
for (cj=0; cj<dp->Nf; ++cj) { /* freq iteration */
//dpow=p[0]*pow(dp->freqs[cj]/dp->ref_freq,p[1]);
fratio=log(dp->freqs[cj]/dp->ref_freq);
dpow=exp(log(p[0])+p[1]*fratio);
sincos(dp->bpa[cj],&sbpa,&cbpa);
invbmaj=1.0/dp->bmaj[cj];
invbmin=1.0/dp->bmin[cj];
for (ci=0; ci<N; ++ci) { /* pixel iteration */
ll=(-(dp->parr[ci].l-*(dp->ll))*sbpa+(dp->parr[ci].m-*(dp->mm))*cbpa)*invbmaj;
mm=(-(dp->parr[ci].l-*(dp->ll))*cbpa-(dp->parr[ci].m-*(dp->mm))*sbpa)*invbmin;
/* sI=I_0 *(f/f0)^{sP} */
x[ck++]+=dpow*exp(-(ll*ll+mm*mm));
}
}
}
