/* p. 165, 25.6 */
double SunRightAscensionRad( double centuryTime)
{
double omegaRad = OmegaRad(centuryTime);
double oc = ObliquityCorrectionEx( centuryTime, omegaRad);
double al = ApparentLongitudeSunEx( centuryTime, omegaRad);
return atan2(cosd(oc) * sind(al), cosd(al));
}
static void astro_sunpos(double d, double *lon, double *r)
{
double M, /* Mean anomaly of the Sun */
w, /* Mean longitude of perihelion */
/* Note: Sun's mean longitude = M + w */
e, /* Eccentricity of Earth's orbit */
E, /* Eccentric anomaly */
x, y, /* x, y coordinates in orbit */
v; /* True anomaly */
/* Compute mean elements */
M = astro_revolution(356.0470 + 0.9856002585 * d);
w = 282.9404 + 4.70935E-5 * d;
e = 0.016709 - 1.151E-9 * d;
/* Compute true longitude and radius vector */
E = M + e * RADEG * sind(M) * (1.0 + e * cosd(M));
x = cosd(E) - e;
y = sqrt(1.0 - e*e) * sind(E);
*r = sqrt(x*x + y*y); /* Solar distance */
v = atan2d(y, x); /* True anomaly */
*lon = v + w; /* True solar longitude */
if (*lon >= 360.0) {
*lon -= 360.0; /* Make it 0..360 degrees */
}
}
void sunpos( double d, double *lon, double *r )
/******************************************************/
/* Computes the Sun's ecliptic longitude and distance */
/* at an instant given in d, number of days since */
/* 2000 Jan 0.0. The Sun's ecliptic latitude is not */
/* computed, since it's always very near 0. */
/******************************************************/
{
double M, /* Mean anomaly of the Sun */
w, /* Mean longitude of perihelion */
/* Note: Sun's mean longitude = M + w */
e, /* Eccentricity of Earth's orbit */
E, /* Eccentric anomaly */
x, y, /* x, y coordinates in orbit */
v; /* True anomaly */
/* Compute mean elements */
M = revolution( 356.0470 + 0.9856002585 * d );
w = 282.9404 + 4.70935E-5 * d;
e = 0.016709 - 1.151E-9 * d;
/* Compute true longitude and radius vector */
E = M + e * RADEG * sind(M) * ( 1.0 + e * cosd(M) );
x = cosd(E) - e;
y = sqrt( 1.0 - e*e ) * sind(E);
*r = sqrt( x*x + y*y ); /* Solar distance */
v = atan2d( y, x ); /* True anomaly */
*lon = v + w; /* True solar longitude */
if ( *lon >= 360.0 )
*lon -= 360.0; /* Make it 0..360 degrees */
}
void rotate_z(vector *v,double theta)
{
double xNew,yNew;
xNew = v->x*cosd(theta)+v->y*sind(theta);
yNew = -v->x*sind(theta)+v->y*cosd(theta);
v->x = xNew; v->y = yNew;
}
void rotate_y(vector *v,double theta)
{
double xNew,zNew;
zNew = v->z*cosd(theta)+v->x*sind(theta);
xNew = -v->z*sind(theta)+v->x*cosd(theta);
v->x = xNew; v->z = zNew;
}
/* -------------------------------------------------------------------------- */
void set_proj()
{
double psx;
double cell;
double cell2;
double r;
double phictr;
dddd = cntrj0 * pj.cds;
sign = (pj.cenlat >= 0.0) ? 1.0 : -1.0;
xn = 0.0;
psi1 = 0.0;
pole = sign*90.0;
psi0 = (pole - pj.cenlat) * DEG_TO_RAD;
if (pj.code == LAMBERT)
{
xn = log10(cosd(pj.stdlat1)) - log10(cosd(pj.stdlat2));
xn = xn/(log10(tand(45.0-sign*pj.stdlat1*0.50)) -
log10(tand(45.0-sign*pj.stdlat2*0.50)));
psi1 = (90.0-sign*pj.stdlat1) * DEG_TO_RAD;
psi1 = sign*psi1;
}
else if (pj.code == POLAR)
{
xn = 1.0;
psi1 = (90.0-sign*pj.stdlat1) * DEG_TO_RAD;
psi1 = sign*psi1;
}
if (pj.code != MERCATOR)
{
psx = (pole-pj.cenlat) * DEG_TO_RAD;
if (pj.code == LAMBERT)
{
cell = RADIUS_EARTH*sin(psi1)/xn;
cell2 = tan(psx*0.50) / tan(psi1*0.50);
}
else
{
cell = RADIUS_EARTH*sin(psx)/xn;
cell2 = (1.0 + cos(psi1))/(1.0 + cos(psx));
}
r = cell*pow(cell2,xn);
xcntr = 0.0;
ycntr = -r;
xc = 0.0;
yc = -RADIUS_EARTH/xn*sin(psi1)*pow(tan(psi0*0.5)/tan(psi1*0.5),xn);
}
else {
c2 = RADIUS_EARTH*cos(psi1);
xcntr = 0.0;
phictr = pj.cenlat * DEG_TO_RAD;
cell = cos(phictr)/(1.0+sin(phictr));
ycntr = -c2*log(cell);
xc = xcntr;
yc = ycntr;
}
return;
}
void fldpnt(double rrho,double rlat,double rlon,double ral,
double rel,double r,double *frho,double *flat,
double *flon) {
double rx,ry,rz,sx,sy,sz,tx,ty,tz;
double sinteta;
/* convert from global spherical to global cartesian*/
sinteta=sind(90.0-rlat);
rx=rrho*sinteta*cosd(rlon);
ry=rrho*sinteta*sind(rlon);
rz=rrho*cosd(90.0-rlat);
sx=-r*cosd(rel)*cosd(ral);
sy=r*cosd(rel)*sind(ral);
sz=r*sind(rel);
tx = cosd(90.0-rlat)*sx + sind(90.0-rlat)*sz;
ty = sy;
tz = -sind(90.0-rlat)*sx + cosd(90.0-rlat)*sz;
sx = cosd(rlon)*tx - sind(rlon)*ty;
sy = sind(rlon)*tx + cosd(rlon)*ty;
sz = tz;
tx=rx+sx;
ty=ry+sy;
tz=rz+sz;
/* convert from cartesian back to global spherical*/
*frho=sqrt((tx*tx)+(ty*ty)+(tz*tz));
*flat=90.0-acosd(tz/(*frho));
if ((tx==0) && (ty==0)) *flon=0;
else *flon=atan2d(ty,tx);
}
// Input x, y ,z and the angle vector
bool ikine(vector<double> *coords, vector<double> *angles, int grip) {
//cout << endl << "Andy Ikine says, hello world" << endl << endl;
double x = coords->at(0);
double y = coords->at(1);
double z = coords->at(2) - L1;
// calculate the closes reach
double reach_limit = L1*cosd(90) + L2*cosd(90 + (asin((-L1*sind(90))/L2) * 180 / M_PI) );
//cout << "Min reach: " << reach_limit << endl;
// Note that the dynamixel and rotate 150(degree) from origin
double magnitude = sqrt( pow(x,2) + pow(y,2) + pow(z,2) );
#if DEBUG
cerr << "Resultant Vector: " << magnitude << endl;
#endif
if ( magnitude > (L2 + L3) ) {
cerr << "Out of reach" << endl;
//return false;
}
// (horizontal, vertical) theta A
angles->at(0) = atan2(x,y);
// converted hypotenuse as the new y value
y = y / cos(angles->at(0));
// equation from the online source
double temp1 = (pow(y,2) + pow(z,2) - pow(L2,2) - pow(L3,2)) / (2 * L2 * L3);
double temp2 = -sqrt( 1 - pow(temp1, 2) );
// theta C
angles->at(2) = atan2( temp2, temp1);
double k1 = L2 + L3 * cos(angles->at(2));
double k2 = L3 * sin(angles->at(2));
// theta B
angles->at(1) = atan2( z, y ) - atan2( k2, k1 );
angles->at(2) += M_PI / 2 - M_PI / 6;
angles->at(1) -= M_PI / 2;
// open : close
angles->at(3) = grip ? 1.3 : -1.4;//(-GRIP_ANGLE / 180.0 * M_PI) ;
//print_values(angles);
return check_angle_range(angles);
}
void iterate(int value)
{
int j, k;
// ITERATIONS_PER_SEC is used to divide
const double ITERATIONS_PER_SEC = 1000.0 / value;
// call this function again in value milliseconds
glutTimerFunc(value, iterate, value);
// detect and resolve collisions
// note: for simplicity, we assume that there are only 2 robots in the ring
for(j = 1; j < robots_size; j++)
{
for(k = 0; k < j; k++)
{
double avg_width = (robots[j].width + robots[k].width) / 2.0;
// if there's a collision between robots[j] and robots[k]
if(magnitude(robots[j].pos.x - robots[k].pos.x, robots[j].pos.y - robots[k].pos.y) < avg_width)
{
point_t robot_j_corner;
double half_j_width = robots[j].width / 2.0;
robot_j_corner = offset_from_robot(&robots[j], half_j_width, half_j_width);
if(point_in_robot(&robots[k], &robot_j_corner))
{
// place code to figure out where the robots hit, now that we know there's a collision
}
}
}
}
// update robots positions and angle (with the velocity and angular velocity)
for(j = 0; j < robots_size; j++)
{
// If the robot's angular velocity is great enough, use a more accurate model
if(fabs(robots[j].rotv) < 10.0)
{
robots[j].pos.x += robots[j].v * cosd(robots[j].rot) / ITERATIONS_PER_SEC;
robots[j].pos.y += robots[j].v * sind(robots[j].rot) / ITERATIONS_PER_SEC;
}
else
{
robots[j].pos.x += robots[j].v * 180.0 / M_PI
* (sind(robots[j].rotv / ITERATIONS_PER_SEC + robots[j].rot)
- sind(robots[j].rot)) / robots[j].rotv;
robots[j].pos.y += robots[j].v * 180.0 / M_PI
* (-cosd(robots[j].rotv / ITERATIONS_PER_SEC
+ robots[j].rot) + cosd(robots[j].rot)) / robots[j].rotv;
}
robots[j].rot += robots[j].rotv / ITERATIONS_PER_SEC;
}
}
void ObservationPointSet(long double NS,long double EW,long double ro)
{
if (NS < -90) NS = -90;
if (NS > 90) NS = -90;
if (EW < -180) EW = -180;
if (EW > 180) EW = 180;
longitude = EW;
LAT = NS - 0.19241666667 * sind(NS * 2); /* 天文緯度 */
RLT = ((0.998327112 + 0.001676399 * cosd(NS * 2) - 0.000003519 * cosd(NS * 4)) * 6378140 + ro) / 6371012;
if (RLT < 0) RLT = 0;
return;
}
QVector3D rotateXYZ(QVector3D vector, QVector3D rotation) {
float x, y, z, x_ = vector.x(), y_ = vector.y(), z_ = vector.z();
//Rotation autour de X
x = x_, y = y_ * cosd(rotation.x()) - z_ * sind(rotation.x()), z = y_ * sind(rotation.x()) + z_ * cosd(rotation.x());
//Rotation autour de Y
x_ = x * cosd(rotation.y()) + z * sind(rotation.y()), y_ = y, z_ = z * cosd(rotation.y()) - x * sind(rotation.y());
//Rotation autour de Z
x = x_ * cosd(rotation.z()) - y_ * sind(rotation.z()), y = x_ * sind(rotation.z()) + y_ * cosd(rotation.z()), z = z_;
return QVector3D(x, y, -z);
}
int sphpad(
int nfield,
double lng0,
double lat0,
const double dist[],
const double pa[],
double lng[],
double lat[])
{
int i;
double eul[5];
/* Set the Euler angles for the coordinate transformation. */
eul[0] = lng0;
eul[1] = 90.0 - lat0;
eul[2] = 0.0;
eul[3] = cosd(eul[1]);
eul[4] = sind(eul[1]);
for (i = 0; i < nfield; i++) {
/* Latitude in the new frame is obtained from angular distance. */
lat[i] = 90.0 - dist[i];
/* Longitude in the new frame is obtained from position angle. */
lng[i] = -pa[i];
}
/* Transform field points to the old system. */
sphx2s(eul, nfield, 0, 1, 1, lng, lat, lng, lat);
return 0;
}
int sphdpa(
int nfield,
double lng0,
double lat0,
const double lng[],
const double lat[],
double dist[],
double pa[])
{
int i;
double eul[5];
/* Set the Euler angles for the coordinate transformation. */
eul[0] = lng0;
eul[1] = 90.0 - lat0;
eul[2] = 0.0;
eul[3] = cosd(eul[1]);
eul[4] = sind(eul[1]);
/* Transform field points to the new system. */
sphs2x(eul, nfield, 0, 1, 1, lng, lat, pa, dist);
for (i = 0; i < nfield; i++) {
/* Angular distance is obtained from latitude in the new frame. */
dist[i] = 90.0 - dist[i];
/* Position angle is obtained from longitude in the new frame. */
pa[i] = -pa[i];
if (pa[i] < -180.0) pa[i] += 360.0;
}
return 0;
}
/*
** Hapgood defines a transformation between GSE and HEE in his 1992
** paper (section 6), but this part isn't online.
**
** The gist of it is, we rotate 180 degrees about Z, and then translate
** along X.
**
** But we also need to add "R", a constant vector defined by
**
** R = [ Rsun, 0, 0 ]
**
** where
**
** r0 (1 - e^2)
** Rsun = ------------
** 1 + e cos(v)
**
** r0 = 1.495985E8 km mean distance of the Sun from Earth.
**
** e = 0.016709 - 0.0000418T0 eccentricity of the Sun's apparent
** orbit around the Earth.
**
** w = 282.94 + 1.72 T0 longitude of perigee of that orbit
**
** v = lambda0 - w (see lambda0 above)
**
**
** Implemented by Ed Santiago, Updated by Kristi Keller
*/
int
gse_twixt_hee(const double et, Vec v_in, Vec v_out, Direction direction)
{
Mat mat;
double r0,e, w,v, Rsun;
hapgood_matrix(180, Z, mat);
/*
** Note that there's no transposition here if the direction is "back";
** the operation works identically in both directions.
*/
mat_times_vec(mat, v_in, v_out);
/* Translate the X axis about the earth-sun distance */
r0 = (double)1.495985e8;
e = 0.016709 - 0.0000418*T0(et);
w = 282.94 + 1.72*T0(et);
v = lambda0(et) - w;
Rsun = r0*(1-e*e)/(1.+e*cosd(v));
/* v_out[0] += (double)1.5e8; */
v_out[0] += Rsun;
return 0;
}
/* -------------------------------------------------------------------------- */
void latlon_to_ij(float latitude, float longitude, float *ri, float *rj)
{
double cell;
double ylon;
double flp;
double psx;
double xx = 0;
double yy = 0;
double r;
if (! is_init)
{
fprintf(stderr, "Using latlon_to_ij without projection init !!!\n");
*ri = -999;
*rj = -999;
return;
}
if (pj.code == MERCATOR)
{
if (latitude != -90.0)
{
cell = cosd(latitude)/(1.0+sind(latitude));
yy = -c2*log(cell);
xx = c2*((longitude-pj.cenlon)*DEG_TO_RAD);
if (pj.cenlon > 0.0 && xx < -dddd)
{
xx = xx + 2.0*c2*((180.0+pj.cenlon)*DEG_TO_RAD);
}
else if (pj.cenlon < 0.0 && xx > dddd)
{
xx = xx - c2*(360.0*DEG_TO_RAD);
}
}
}
else
{
ylon = longitude - pj.cenlon;
if (ylon > 180.0) ylon -= 360.0;
if (ylon < -180.0) ylon += 360.0;
flp = xn*(ylon*DEG_TO_RAD);
psx = (pole - latitude) * DEG_TO_RAD;
r = -RADIUS_EARTH/xn*sin(psi1)*pow(tan(psx*0.50)/tan(psi1*0.50),xn);
if (pj.cenlat < 0)
{
xx = r*sin(flp);
yy = r*cos(flp);
}
else
{
xx = -r*sin(flp);
yy = r*cos(flp);
}
}
*ri = (xx - xc) / pj.ds + cntrj;
*rj = (yy - yc) / pj.ds + cntri;
return;
}
/******************************************************
* get_tv:
* Returns a "target state vector", containing the
* position and velocity of the given point on the
* earth's surface.*/
stateVector get_tv(double RE,double RP,double lat,double lon)
{
double Rn;
stateVector v;
double ecc2=1-RP*RP/(RE*RE);
Rn = RE/sqrt(1-ecc2*SQR(sind(lat)));
v.pos.x = (Rn)*cosd(lon)*cosd(lat);
v.pos.y = (Rn)*sind(lon)*cosd(lat);
v.pos.z = (Rn*(1-ecc2))*sind(lat);
/*fixed2gei with gha 0.0 simply sets the velocity
of the target point to the rotation rate of the earth.*/
v.vel.x=v.vel.y=v.vel.z=0.0;
fixed2gei(&v,0.0);
return v;
}
/*
** The HAE to HEEQ transformation is given by the matrix
**
** S2 = <theta0,Z>*<i,X>*<Omega,Z>
**
** where the rotation angle theta0 is the the longitude of the Sun's
** central meridian, i is the the inclination of the Sun's equator and
** Omega is the the ecliptic longitude of the ascending node of the Sun's
** equator. This transformation comprises a rotation in the plane of the
** ecliptic from the First Point of Aries to the ascending node of the
** solar equator, then a rotation from the plane of the ecliptic to the
** plane of the equator and finally a rotation in the plane of the solar
** equator from the ascending node to the central meridian.
**
** Implemented by Kristi Keller on 2/2/2004
*/
void
mat_S2(const double et, Mat mat)
{
Mat mat_tmp;
double Omega = 73.6667+0.013958*(MJD(et)+3242)/365.25;
double theta0 = atand(cosd(7.25) * tand(lambda0(et)-Omega));
double angle_1 = lambda0(et)-Omega;
angle_1=fmod(angle_1,360.0);
if (angle_1 < 0.0) angle_1+=360.0;
theta0=fmod(theta0,360.0);
if (theta0 < 0.0) theta0+=360.0;
if (angle_1 < 180.0) {
if (theta0 < 180.0) theta0+=180.0;
}
if (angle_1 > 180.0) {
if (theta0 > 180.0) theta0-=180.0;
}
hapgood_matrix(theta0, Z, mat);
hapgood_matrix(7.25, X, mat_tmp);
mat_times_mat(mat, mat_tmp, mat);
hapgood_matrix(Omega, Z, mat_tmp);
mat_times_mat(mat, mat_tmp, mat);
}
void geocnvrt(double gdlat,double gdlon,
double xal,double xel,double *ral,double *rel) {
double kxg,kyg,kzg,kxr,kyr,kzr;
double rrad,rlat,rlon,del;
kxg=cosd(xel)*sind(xal);
kyg=cosd(xel)*cosd(xal);
kzg=sind(xel);
geodtgc(1,&gdlat,&gdlon,&rrad,&rlat,&rlon,&del);
kxr=kxg;
kyr=kyg*cosd(del)+kzg*sind(del);
kzr=-kyg*sind(del)+kzg*cosd(del);
*ral=atan2d(kxr,kyr);
*rel=atand(kzr/sqrt((kxr*kxr)+(kyr*kyr)));
}
/* FUNCTION: Quaternion :: GenerateFromEuler
ARGUMENTS: roll (degrees)
pitch (degrees)
yaw (degrees)
RETURN: n/a
DESCRIPTION: Creates quaternion from euler angles. Borowed from Bullet
*/
void Quaternion :: GenerateFromEuler(float roll, float pitch, float yaw)
{
float halfYaw = 0.5f * yaw;
float halfPitch = 0.5f * pitch;
float halfRoll = 0.5f * roll;
float cosYaw = cosd(halfYaw);
float sinYaw = sind(halfYaw);
float cosPitch = cosd(halfPitch);
float sinPitch = sind(halfPitch);
float cosRoll = cosd(halfRoll);
float sinRoll = sind(halfRoll);
x = sinRoll*cosPitch*cosYaw - cosRoll*sinPitch*sinYaw;
y = cosRoll*sinPitch*cosYaw + sinRoll*cosPitch*sinYaw;
z = cosRoll*cosPitch*sinYaw - sinRoll*sinPitch*cosYaw;
w = cosRoll*cosPitch*cosYaw + sinRoll*sinPitch*sinYaw;
}
Geometry Geometry::fromDisc(float w, float h, int e) //static
{
if (e<3) throw GeometryError("fromDisc: Must have at least 3 edges!");
Geometry geo;
Geometry::Triangle tri;
Geometry::Vertex vert;
const float dDeg = 360.f/float(e);
w *= 0.5;
h *= 0.5;
vert.pos = Vec3{0.f, 0.f, 0.f};
vert.norm = Vec3{0.f, 0.f, 1.f};
vert.tex = Vec2{0.5f, 0.5f};
geo.vertices.push_back(vert);
tri[0] = 0;
tri[1] = 0;
tri[2] = 0;
float deg = -dDeg;
vert.pos = Vec3{cosd(deg)*w, sind(deg)*h, 0.f};
vert.tex = Vec2{cosd(deg), sind(deg)};
tri[1] = 1;
geo.vertices.push_back(vert);
for (int i=0; i<e-1; ++i)
{
deg = dDeg*i;
vert.pos = Vec3{cosd(deg)*w, sind(deg)*h, 0.f};
vert.tex = Vec2{cosd(deg), sind(deg)};
tri[2] = geo.vertices.size();
geo.vertices.push_back(vert);
geo.triangles.push_back(tri);
tri[1] = tri[2];
}
tri[2] = 1;
geo.triangles.push_back(tri);
return geo;
}
matrix MatrixRotateZ (float theta) {
float s = sind (theta);
float c = cosd (theta);
return matrix (c, s, 0, 0,
-s, c, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
}
/* FUNCTION: Quaternion :: GenerateLocalRotation
ARGUMENTS: angle
x_axis, y_axis, z_axis axis of rotation
RETURN: n/a
DESCRIPTION: Create unit quaterion corresponding to a rotation through angle <angle>
about axis <a,b,c>.
q = cos (angle/2) + A(a,b,c)*sin(angle/2)
*/
void Quaternion :: GenerateLocalRotation(float angle, float x_axis, float y_axis, float z_axis)
{
float temp = sind(0.5f*angle);
w = cosd(0.5f*angle);
x = x_axis * temp;
y = y_axis * temp;
z = z_axis * temp;
}
float Crystal_dSpacing (Crystal_Struct* crystal, int i_miller, int j_miller, int k_miller) {
Crystal_Struct* cc;
if (i_miller == 0 && j_miller == 0 && k_miller == 0) return 0;
cc = crystal; /* Just for an abbreviation. */
return (cc->volume / (cc->a * cc->b * cc->c)) * sqrt(1 / (
pow2(i_miller * sind(cc->alpha) / cc->a) + pow2(j_miller * sind(cc->beta) / cc->b) +
pow2(k_miller * sind(cc->gamma) / cc->c) +
2 * i_miller * j_miller * (cosd(cc->alpha) * cosd(cc->beta) - cosd(cc->gamma)) / (cc->a * cc->b) +
2 * i_miller * k_miller * (cosd(cc->alpha) * cosd(cc->gamma) - cosd(cc->beta)) / (cc->a * cc->c) +
2 * j_miller * k_miller * (cosd(cc->beta) * cosd(cc->gamma) - cosd(cc->alpha)) / (cc->b * cc->c)));
}
static inline void CorrectHKLsLatC(double LatC[6],double Lsd,double Wavelength,double hkls[5000][4],double Thetas[5000],double hklsIn[5000][4],int nhkls)
{
double a=LatC[0],b=LatC[1],c=LatC[2],alpha=LatC[3],beta=LatC[4],gamma=LatC[5];
int hklnr;
for (hklnr=0;hklnr<nhkls;hklnr++){
double ginit[3]; ginit[0] = hklsIn[hklnr][0]; ginit[1] = hklsIn[hklnr][1]; ginit[2] = hklsIn[hklnr][2];
double SinA = sind(alpha), SinB = sind(beta), SinG = sind(gamma), CosA = cosd(alpha), CosB = cosd(beta), CosG = cosd(gamma);
double GammaPr = acosd((CosA*CosB - CosG)/(SinA*SinB)), BetaPr = acosd((CosG*CosA - CosB)/(SinG*SinA)), SinBetaPr = sind(BetaPr);
double Vol = (a*(b*(c*(SinA*(SinBetaPr*(SinG)))))), APr = b*c*SinA/Vol, BPr = c*a*SinB/Vol, CPr = a*b*SinG/Vol;
double B[3][3]; B[0][0] = APr; B[0][1] = (BPr*cosd(GammaPr)), B[0][2] = (CPr*cosd(BetaPr)), B[1][0] = 0,
B[1][1] = (BPr*sind(GammaPr)), B[1][2] = (-CPr*SinBetaPr*CosA), B[2][0] = 0, B[2][1] = 0, B[2][2] = (CPr*SinBetaPr*SinA);
double GCart[3];
MatrixMultF(B,ginit,GCart);
double Ds = 1/(sqrt((GCart[0]*GCart[0])+(GCart[1]*GCart[1])+(GCart[2]*GCart[2])));
hkls[hklnr][0] = GCart[0];hkls[hklnr][1] = GCart[1];hkls[hklnr][2] = GCart[2];
hkls[hklnr][3] = hklsIn[hklnr][3];
Thetas[hklnr] = (asind((Wavelength)/(2*Ds)));
}
}
void Turtle::triangle(Cairo::RefPtr<Cairo::Context> cr, double x, double y, double size, double a) {
cr->save();
// cr->set_source_rgb(0, 0, 0);
/*
if (first) {
first = false;
sand->set_source_rgb(0, 0, 1);
}
*/
cr->move_to(x + size * cosd(a), y + size * sind(a));
cr->line_to(x + size * cosd(a + 120), y + size * sind(a + 120));
cr->line_to(x + size * cosd(a + 240), y + size * sind(a + 240));
cr->fill();
cr->close_path();
cr->stroke();
cr->restore();
colored_area += sqrt(3) / 2 * size * size;
}
void c_bulletController_start(void *pself) {
C_SELF(CBulletController)
CTransform *tx = (CTransform *)entity_getComponent(self->super->entity,
C_TRANSFORM);
if (tx) {
tx->velocity->x = cosd(tx->rotation) * self->speed;
tx->velocity->y = sind(tx->rotation) * self->speed;
}
}
static void geo2topo(double *ra, double *dec, double ha, double par, double lat, double lon)
{
/* ra and dec are geocentric - convert them to topocentric */
double topra, topdec, rho, gclat, g ;
/* Correct for flattening of Earth */
gclat = lat - 0.1924 * sind(2*lat) ;
rho = 0.99883 + 0.00167 * cosd(2*lat) ;
/* Auxiliary angle */
g = F * atan2(tan(gclat), cos(ha)) ;
topra = *ra - par * rho * cosd(gclat) * sind(ha) / cosd(*dec) ;
topdec = *dec - par * rho * sind(gclat) * sind(g - *dec) / sind(g) ;
*ra = topra ;
*dec = topdec ;
}
point_t offset_from_robot(robot_t * robot, double x, double y)
{
point_t result;
double rcosd = cosd(robot->rot);
double rsind = sind(robot->rot);
result.x = robot->pos.x + rcosd * y + rsind * x;
result.y = robot->pos.y + -rcosd * x + rsind * y;
return result;
}
void Search::getCoord(double disp, double heading) {
/*
Inputs: (pass by reference)
* phi = current latitude [deg]
* lam = current longitude [deg]
* disp = desired displacement distance [meters]
* heading = desired direction of displacement [deg]
* Pass in values for displacements in N, S, E, W
* 0 - displacement N
* 90 - displacement E
* 180 - displacement S
* 270 - displacement W
*
Outputs:
* phi and lam will be changed to reflect new long and lat location after displacement in specified heading direction
*/
//FCC cites that Flat earth approximation breaks down past 475km (CFR 73.208) but autonomous mission shouldnt span more than 100 km range
if (disp > 100000) {
return;
}
//declare parameters to store linear distance per degree of longitude and latitude
double Lphi, Llam;
double phi = next_point.lat;
// PHI = degrees of LATITUDE
// LAM = degrees of LONGITUDE
//uncomment section below for speed at cost of accuracy
/*
// <TODO: Implement smaller ranges of phi values to improve accuracy of estimation>
// current accuracy within +/- 0.3 meters (~1ft)
// if (33.5 <= phi && phi < 34.5) {
// Lphi = 110922; // [m]
// Llam = 92385;
//
// } else if (34.5 <= phi && phi < 35.5) {
// Lphi = 110941; // [m]
// Llam = 91288.4;
//
// } else if (35.5 <= phi && phi < 36.5) {
// Lphi = 110959; // [m]
// Llam = 90163.9;
//
// } else {
// default case if not within a predetermined latitude angle
// Lphi = 111132.92 - 559.82 * cosd((2 * phi)) + 1.175 * cosd((4 * phi))
// - 0.0023 * cosd((6 * phi));
// Llam = 111412.84 * cosd(phi) - 93.5 * cos((3 * phi)) - 0.118 * cosd((5 * phi));
// }
*/
//distance per degree of lat and long calculated each time
Lphi = 111132.92 - 559.82 * cosd((2 * phi)) + 1.175 * cosd((4 * phi))
- 0.0023 * cosd((6 * phi));
Llam = 111412.84 * cosd(phi) - 93.5 * cos((3 * phi)) - 0.118 * cosd((5 * phi));
next_point.lat += disp * cosd(heading) / Lphi; //[deg]
next_point.lon += disp * sind(heading) / Llam; //[deg]
}
void fldpnt_sph(double frho,double flat,double flon,double az,
double r,double *xlat,double *xlon) {
double api,aside,bside,cside;
double Aangl,Bangl,arg;
api=4*atan(1.0);
cside=90.0-flat;
Aangl=az;
if (Aangl > 180) Aangl=Aangl-360;
bside=r/frho*(180.0/api);
arg=cosd(bside)*cosd(cside)+sind(bside)*sind(cside)*cosd(Aangl);
if (arg <= -1.0) arg=-1.0;
if (arg >= 1.0) arg=1.0;
aside=acosd(arg);
arg=(cosd(bside)-cosd(aside)*cosd(cside))/(sind(aside)*sind(cside));
if (arg <= -1.0) arg=-1.0;
if (arg >= 1.0) arg=1.0;
Bangl=acosd(arg);
if (Aangl <0) Bangl=-Bangl;
*xlat=90-aside;
*xlon=flon+Bangl;
if (*xlon < 0) *xlon+=360;
if (*xlon > 360) *xlon-=360;
}
