int kinematicsInverse(const EmcPose * pos, double *joints, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { // B correction double zb = (*(haldata->pivot_length) + pos->w) * cos(d2r(pos->b)); double xb = (*(haldata->pivot_length) + pos->w) * sin(d2r(pos->b)); // C correction double xyr = hypot(pos->tran.x, pos->tran.y); double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c); // U correction double zv = pos->u * sin(d2r(pos->b)); double xv = pos->u * cos(d2r(pos->b)); // V correction is always in joint 1 only joints[0] = xyr * cos(xytheta) - xb + xv; joints[1] = xyr * sin(xytheta) + pos->v; joints[2] = pos->tran.z + zb + zv - *(haldata->pivot_length); joints[3] = pos->a; joints[4] = pos->b; joints[5] = pos->c; joints[6] = pos->u; joints[7] = pos->v; joints[8] = pos->w; return 0; }
cMatrix3d InputManager::GetCameraTransformations() { cMatrix3d cam; //Col0 has Position cam.setCol0(cVector3d(transforms.xPos, transforms.yPos, transforms.zPos)); //Col1 has Gaze dvec4 gazeCamSpace = dvec4( 0, 0, 1, 1); dvec4 gazeWorldSpace = camToWorld * gazeCamSpace; cam.setCol1(cVector3d(gazeWorldSpace.x, gazeWorldSpace.y, gazeWorldSpace.z)); //Col2 has Up cVector3d up; if(transforms.elevation == 0) { //If Looking straight down, change the up vector to the heading up = cVector3d(cos(d2r(transforms.heading)), 0, sin(d2r(transforms.heading))); } else if(transforms.elevation == 180) { //If looking straight up, change the up vector to the negative heading up = cVector3d(0 - cos(d2r(transforms.heading)), 0, 0 - sin(d2r(transforms.heading))); } else { up = cVector3d(0,1,0); } cam.setCol2(up); return cam; }
int main(){ V6 v6; DMS dms; HMS hms; double ra=0.0, de=0.0, pmra=0.0, pmde=0.0, px=0.0, rv=0.0, C=0.0; /* Barnard's star from Hipparcos catalog. ICRS Epoch J1991.25 */ ra = d2r(269.45402305); de = d2r(4.66828815); px = 549.01 / 1000.0; /* Arc seconds */ rv = 0.0; pmra = (-797.84 / 1000.0 ) / cos(de); /* pmra * cos(de) into pmra */ pmra *= 100.0; /* Arcseconds per century. */ pmde = (10326.93 / 1000.0); pmde *= 100.0; /* Arcseconds per century. */ C = CJ; printf("RA %f DE %f PMRA %f PMDE %f PX %f RV %f\n", ra, de, pmra, pmde, px, rv); v6 = v6init(CARTESIAN); v6 = cat2v6(ra, de, pmra, pmde, px, rv, C); v6 = proper_motion(v6, J2000, JYEAR2JD(1991.25)); printf("RAJ2000: %s DEJ2000:%s\n", fmt_alpha(v6GetAlpha(v6c2s(v6))), fmt_d(r2d(v6GetDelta(v6c2s(v6))))); hms = hms2hms(r2hms(v6GetAlpha(v6c2s(v6)))); dms = dms2dms(r2dms(v6GetDelta(v6c2s(v6)))); printf("RAJ2000: hh %.1f mm %.1f ss %.4f\n", hms.hh, hms.mm, hms.ss); printf("DEJ2000: dd %.1f mm %.1f ss %.4f\n", dms.dd, dms.mm, dms.ss); return 0; }
double REIXSXESCalibration::spectrometerTheta(double spectrometerRotationDrive) const { // lh = liftheight double lh = frameH0_ - spectrometerRotationDrive + spectrometerRotDrive0_; double l=frameL_; double a=frameA_; double b=frameB_; double a2 = a*a; double b2 = b*b; double l2 = l*l; double lh2 = lh*lh; double l3 = l2*l; double lh4 = lh2*lh2; double lh6 = lh4*lh2; double l4 = l2*l2; double b4 = b2*b2; double a4 = a2*a2; // phi = theta_m - d2r(frameATheta_) double phi = atan2( -1./2.*(a2-b2-l/(4*l2+4*lh2)*(4*a2*l+4*l*lh2+4*l3-4*b2*l+4*sqrt(-2*l2*lh4-l4*lh2-lh2*b4+2*lh4*b2+2*lh4*a2+2*a2*l2*lh2+2*l2*lh2*b2+2*lh2*b2*a2-a4*lh2-lh6))+l2+lh2)/a/lh, 1./2./(4*l2+4*lh2)*(4*a2*l+4*l*lh2+4*l3-4*b2*l+4*sqrt(-2*l2*lh4-l4*lh2-lh2*b4+2*lh4*b2+2*lh4*a2+2*a2*l2*lh2+2*l2*lh2*b2+2*lh2*b2*a2-a4*lh2-lh6))/a );// one nasty inverse function double theta_m = phi + d2r(frameATheta_); // theta': angle above calibration position = theta - theta_0 [either aligned to working coordianates, or to frame rails. Doesn't matter, since it's a difference between two angles.] // theta_m: angle of inclineable rails above horizontal frame rails = theta' + theta_0m return theta_m + d2r(spectrometerTheta0_) - d2r(spectrometerTheta0m_); }
int kinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { // B correction double zb = (*(haldata->pivot_length) + joints[8]) * cos(d2r(joints[4])); double xb = (*(haldata->pivot_length) + joints[8]) * sin(d2r(joints[4])); // C correction double xyr = hypot(joints[0], joints[1]); double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]); // U correction double zv = joints[6] * sin(d2r(joints[4])); double xv = joints[6] * cos(d2r(joints[4])); // V correction is always in joint 1 only pos->tran.x = xyr * cos(xytheta) + xb - xv; pos->tran.y = xyr * sin(xytheta) - joints[7]; pos->tran.z = joints[2] - zb + zv + *(haldata->pivot_length); pos->a = joints[3]; pos->b = joints[4]; pos->c = joints[5]; pos->u = joints[6]; pos->v = joints[7]; pos->w = joints[8]; return 0; }
void CEnemy3::Exec() { // 移動 if(y <= 150){ y += 1.0f; }else{ // 途中から水平移動に切り替え if(left == true){ x += 1.0f; }else{ x -= 1.0f; } if(x > 704.0f && x < -64.0f){ RemoveObject(this); return; } } // 弾の発射 if(frame % 120 == 0 && frame > 0){ //120フレームごとに連射フラグを切り替える shoot = !shoot; // 連射終了直後であれば // 真下に向かって5way弾を発射する if(shoot == false){ float angle[5] = {90.0f, 70.0f, 50.0f, 110.0f, 130.0f}; for(int i = 0; i < 5; i++){ AppendObject( new CEnemyBullet(x, y + 18, d2r(angle[i]), 2.5f), ENEMYBULLET_PRIORITY, true); } } } // 連射フラグが有効になっているのであれば // 20フレームごとに真下に弾を発射 if(shoot == true){ if(frame % 20 == 0){ AppendObject( new CEnemyBullet(x - 40, y + 37, d2r(90.0f), 2.0f), ENEMYBULLET_PRIORITY, true); AppendObject( new CEnemyBullet(x + 40, y + 37, d2r(90.0f), 2.0f), ENEMYBULLET_PRIORITY, true); } } frame++; sprite.Draw(x, y); if(this->HitTest(player)){ player->Destroy(); this->Damaged(); } }
double refract(double z, double refa, double refb, int flag) { double dz; /* refraction */ double err; /* iteration error */ double tz0; /* tangent value */ double z0; /* refracted (observed) zenith distance */ double z0_last; /* previous value */ int i; /* limit the given zenith angle */ if (z < 0) { z = 0; } else if (z > d2r(Z_LIMIT)) { z = d2r(Z_LIMIT); } if (flag > 0) { /* apply refraction */ /* we have to iterate the refraction equation to get z0 */ /* first guess */ z0 = z; /* use newton's method to find z0 */ for (i = 0; i < ITERATIONS; i++) { z0_last = z0; tz0 = tan(z0); dz = tz0 * (refa + tz0 * (tz0 * refb)); z0 -= ((z0-z) + dz) / (1 + (refa+3*refb*tz0*tz0)/(cos(z0)*cos(z0))); err = fabs(z0-z0_last); #ifdef DEBUG (void)fprintf(stdout, "refract: z0 %.15g dz %.15g z %.15g err %.15g\n", z0, dz, z, err); #endif } dz *= -1.0; } else { /* remove refraction */ z0 = z; tz0 = tan(z0); dz = tz0 * (refa + tz0 * (tz0 * refb)); z = z0 + dz; #ifdef DEBUG (void)fprintf(stdout, "refract: z0 %.15g tz0 %.15g dz %.15g z %.15g\n", z0, tz0, dz, z); #endif } return(dz); }
int main(){ V6 v6; v6 = geod2geoc(d2r(30.567), d2r(46.713), 1500.0); printf("X %.9f \tY %.9f \tZ %.9f \nXDOT %.9f \tYDOT %.9f \tZDOT %.9f\n", v6GetX(v6), v6GetY(v6), v6GetZ(v6), v6GetXDot(v6), v6GetYDot(v6), v6GetZDot(v6)); return 0; }
V6 equ2gal(V6 v6) { /* subtract e-terms */ v6 = ellab(B1950, v6, -1); v6 = m3v6(m3Rz(d2r(GAL_RA)), v6); v6 = m3v6(m3Ry(d2r(90-GAL_DEC)), v6); v6 = m3v6(m3Rz(d2r(90-GAL_LON)), v6); return(v6); }
NuId calcNuStep(const IntegralArea* ia, const unsigned int nu_step) { NuId nuid; real tmp1, tmp2; nuid.nu = ia->nu_min + (nu_step * ia->nu_step_size); tmp1 = d2r(90.0 - nuid.nu - ia->nu_step_size); tmp2 = d2r(90.0 - nuid.nu); nuid.id = mw_cos(tmp1) - mw_cos(tmp2); nuid.nu += 0.5 * ia->nu_step_size; return nuid; }
void draw_dots(SDL_Surface *surface, int secondes, int totalSecondes, int x, int y, int radius, int dotRadius, Uint32 dotColor) { int centerX = x + radius; int centerY = y + radius; secondes++; if(secondes > totalSecondes) { secondes = totalSecondes; } for(int i = 0; i < secondes; i++) { int dotX = centerX + radius * cos((d2r(360)/totalSecondes)*i-d2r(90)); int dotY = centerY + radius * sin((d2r(360)/totalSecondes)*i-d2r(90)); draw_circle(surface, dotX, dotY, dotRadius, dotColor); } }
void InputManager::BuildCamToWorld() { dvec3 gaze, up, v, u, w; gaze.x = sin(d2r(transforms.elevation)) * cos(d2r(transforms.heading)); gaze.y = 0 - cos(d2r(transforms.elevation)); gaze.z = sin(d2r(transforms.elevation)) * sin(d2r(transforms.heading)); if(transforms.elevation == 0) { //If Looking straight down, change the up vector to the heading up = dvec3(cos(d2r(transforms.heading)), 0, sin(d2r(transforms.heading))); } else if(transforms.elevation == 180) { //If looking straight up, change the up vector to the negative heading up = dvec3(0 - cos(d2r(transforms.heading)), 0, 0 - sin(d2r(transforms.heading))); } else { up = dvec3(0,1,0); } w.x = 0 - gaze.x; w.y = 0 - gaze.y; w.z = 0 - gaze.z; //Shouldn't be necessary, but just in case... w = normalize(w); u = cross(up, w); u = normalize(u); v = cross(w, u); v = normalize(v); dmat4x4 changeBase = dmat4x4( u.x, u.y, u.z, 0, v.x, v.y, v.z, 0, gaze.x, gaze.y, gaze.z, 0, 0 , 0 , 0 , 1); changeBase = transpose(changeBase); dmat4x4 eye2origin = dmat4x4( 1, 0, 0, 0 - transforms.xPos, 0, 1, 0, 0 - transforms.yPos, 0, 0, 1, 0 - transforms.zPos, 0, 0, 0, 1); eye2origin = transpose(eye2origin); dmat4x4 changeBaseINV = inverse(changeBase); dmat4x4 eye2originINV = inverse(eye2origin); rotateToWorld = changeBaseINV; camToWorld = eye2originINV * changeBaseINV; }
void CBoss::Action2() { // 左右に移動 if(moveflag1 == 0){ x -= 0.5f; if(x < 100.0f) moveflag1 = 1; }else{ x += 0.5f; if(x > 540.0f) moveflag1 = 0; } frame++; if(frame % 40 == 0){ ShootFromNozzle(2.0f); } if(frame == 240){ // 中央部分から拡散弾 float range[] = {30.0f, 60.0f, 90.0f, 120.0f, 150.0f}; for(int i = 0; i < 5; i++){ AppendObject( new CEnemyBullet(x, y + 50.0f, d2r(range[i]), 3.0f), ENEMYBULLET_PRIORITY, true); } frame = 0; } }
void rotz(mat4 m, float a) { const float r=d2r(a), c=cosf(r), s=sinf(r); vec4 t[2]; t[0]=m[0]; t[1]=m[1]; m[0]=(t[0]*c)+(t[1]*-s); m[1]=(t[0]*s)+(t[1]*c); }
double do_sind(double x) { double temp; errno = 0; temp = sin(d2r(x)); MATH_ERROR("sind"); return (temp); }
double do_cosd(double x) { double temp; errno = 0; temp = cos(d2r(x)); MATH_ERROR("cosd"); return (temp); }
double do_tand(double x) { double temp; errno = 0; temp = tan(d2r(x)); MATH_ERROR("tand"); return (temp); }
int camera_sphere(double f,double v,double h,double o){ int x,y; double a_v=r2d(2*atan(v/(2*f))); double a_h=r2d(2*atan(h/(2*f))); verbose(L_CAMR,"%s: focal: %6.2f mm",cam->name,f); verbose(L_CAMR,"%s: υ: sensor: %6.2f • %6.2f mm",cam->name,v,h); verbose(L_CAMR,"%s: υ: α: %6.2f • %6.2f °",cam->name,a_v,a_h); a_v=a_v*(1-(o/2));a_h=a_h*(1-(o/2)); verbose(L_CAMR,"%s: υ: α': %6.2f • %6.2f °",cam->name,a_v,a_h); verbose(L_CAMR,"%s: υ: Ξ: %6.3f",cam->name,o); for(y=0;y<=ceil(180/a_v);y++){ double pos_theta=(y*(180/ceil(180/a_v)))-90; for(x=0;x<ceil(360/(a_h/cos(d2r(pos_theta))));x++){ double pos_phi=x*(360/ceil(360/(a_h/cos(d2r(pos_theta))))); go(0,pos_theta,pos_phi); trigger_shot(0); } } }
int main(){ double ra = (12+22/60.0+54.899/3600.0) * (2*M_PI/24.0); double de = (15+49/60.0+20.57/3600.0) * (2*M_PI/360.0); double ra1, ra1_d, de1, de1_d; double ep = J2000; double eq = J2000; DMS ra_dms, de_dms; V6 v6; V6 pvec[N_TPM_STATES]; TPM_TSTATE tstate; int s1 = TPM_S06; /* Heliocentric mean J2000 FK5 ~~ ICRS */ int s2 = TPM_S00; /* Assign required states. */ for(int i=TPM_S00; i < N_TPM_STATES; i ++){ tpm_data(&tstate, TPM_INIT); tstate.utc = J2000; tstate.lon = d2r(-111.598333); tstate.lat = d2r(31.956389); tstate.alt = 2093.093; tstate.delta_ut = delta_UT(tstate.utc); tpm_data(&tstate, TPM_ALL); v6 = v6init(SPHERICAL); v6SetR(v6, 1e9); v6SetAlpha(v6, ra); v6SetDelta(v6, de); pvec[s1] = v6s2c(v6); s2 = i; tpm(pvec, s1, s2, ep, eq, &tstate); v6 = v6c2s(pvec[s2]); ra_dms = dms2dms(r2dms(v6GetAlpha(v6))); de_dms = dms2dms(r2dms(v6GetDelta(v6))); printf("%02d-%02d %7.2f %7.2f %8.4f %7.2f %7.2f %8.4f\n", s1, s2, ra_dms.dd, ra_dms.mm, ra_dms.ss, de_dms.dd, de_dms.mm, de_dms.ss); } return 0; }
int main(){ V6 v6; #define L 3 double ha, dec; double az[L] = {0, 90, 133.30805555555557}; double el[L] = {90, -45, 59.086111111111116}; v6 = v6init(SPHERICAL); v6SetR(v6, 1e9); for (int i=0; i < L; i++){ v6SetAlpha(v6, d2r(az[i])); v6SetDelta(v6, d2r(el[i])); v6 = v6c2s(azel2hadec(v6s2c(v6), d2r(43.07833))); ha = r2d(r2r(v6GetAlpha(v6))); dec = r2d(r2r(v6GetDelta(v6))); printf("%.9f %.9f\n", ha, dec); } return 0; }
void persp(mat4 m, float fov, float a, float n, float f) { const float nf=1.0f/(n-f); memset((void*)m,0,_mat4s); m[1][1]=1.0f/tanf(d2r(fov/2.0f)); m[0][0]=m[1][1]/a; m[2][2]=(n+f)*nf; m[2][3]=-1.0f; m[3][2]=2.0f*n*f*nf; }
NuConstants* prepareNuConstants(unsigned int nu_steps, real nu_step_size, real nu_min) { unsigned int i; real tmp1, tmp2; NuConstants* nu_consts; nu_consts = (NuConstants*) mwMallocA(sizeof(NuConstants) * nu_steps); for (i = 0; i < nu_steps; ++i) { nu_consts[i].nu = nu_min + (i * nu_step_size); tmp1 = d2r(90.0 - nu_consts[i].nu - nu_step_size); tmp2 = d2r(90.0 - nu_consts[i].nu); nu_consts[i].id = mw_cos(tmp1) - mw_cos(tmp2); nu_consts[i].nu += 0.5 * nu_step_size; } return nu_consts; }
int camera_vshot(double f,double v,double h,double o,double vv,double vh){ int x,y; double a_v=r2d(2*atan( v/(2*f))); double a_h=r2d(2*atan( h/(2*f))); double va_v=r2d(2*atan(vv/(2*f))); double va_h=r2d(2*atan(vh/(2*f))); verbose(L_CAMR,"%s: focal: %6.2f mm",cam->name,f); verbose(L_CAMR,"%s: υ: sensor: %6.2f • %6.2f mm",cam->name,v,h); verbose(L_CAMR,"%s: υ: α: %6.2f • %6.2f °",cam->name,a_v,a_h); a_v=a_v*(1-(o/2));a_h=a_h*(1-(o/2)); verbose(L_CAMR,"%s: υ: α': %6.2f • %6.2f °",cam->name,a_v,a_h); verbose(L_CAMR,"%s: υ: Ξ: %6.3f",cam->name,o); verbose(L_CAMR,"%s: ν: sensor: %6.2f • %6.2f mm",cam->name,vv,vh); verbose(L_CAMR,"%s: ν: α: %6.2f • %6.2f °",cam->name,va_v,va_h); for(y=0;y<=floor(va_v/a_v);y++){ double pos_theta=((a_v*((y*2)-floor(va_v/a_v)))/2); for(x=0;x<=floor(va_h/(a_h/cos(d2r(pos_theta))));x++){ double pos_phi=(((a_h/cos(d2r(pos_theta)))*((x*2)-floor(va_h/(a_h/cos(d2r(pos_theta))))))/2); go(0,pos_theta,(y%2?-pos_phi:pos_phi)); trigger_shot(0); } } }
double tdt2tdb(double tdt) { double T; /* elapsed julian centuries */ double dt; /* (tdb - tdt) in seconds */ double g; double tdb; T = (tdt - J2000) / 36525; g = d2r(357.528 + (T * 35999.050)); dt = 0.001658 * sin(g + (0.01671 * sin(g))); tdb = tdt + (dt / 86400); return(tdb); }
int main(){ V6 v6; v6 = v6init(SPHERICAL); v6SetR(v6, 1e9); v6SetAlpha(v6, d2r(34.1592)); v6SetDelta(v6, d2r(12.9638)); v6SetRDot(v6, -0.123); v6SetAlphaDot(v6, 0.382); v6SetDeltaDot(v6, 1.0); v6 = v6s2c(v6); v6 = precess(J2000, J1984, v6, PRECESS_FK5); v6 = v6c2s(v6); printf("R %.10f \tALPHA %.10f \tDELTA %.10f \nRDOT %.10f \tALPHADOT %.10f \tDELTADOT %.10f\n", v6GetR(v6), v6GetAlpha(v6), v6GetDelta(v6), v6GetRDot(v6), v6GetAlphaDot(v6), v6GetDeltaDot(v6)); return 0; }
void setup_obliquity( JULIAN *ptr ) { double temp, eps; temp = ( -1.81e-3 * ptr->jd_cent ) + 5.9e-3; temp *= ptr->jd_cent; temp += 4.6845e1; temp *= ptr->jd_cent; eps = 2.345229444e1 - ( temp / 3600.0 ); obliquity = eps; eps = d2r( eps ); sin_obliquity = _sin( eps ); cos_obliquity = _cos( eps ); tan_obliquity = _tan( eps ); }
int main(){ V6 v6; v6 = v6init(CARTESIAN); v6SetX(v6, 0.5); v6SetY(v6, 0.173611298); v6SetZ(v6, 0.84844511); v6SetXDot(v6, -0.034000000); v6SetYDot(v6, 0.251873488); v6SetZDot(v6, -0.872330067); v6 = equ2ecl(v6, d2r(23.7)); printf("X %.9f \tY %.9f \tZ %.9f \nXDOT %.9f \tYDOT %.9f \tZDOT %.9f\n", v6GetX(v6), v6GetY(v6), v6GetZ(v6), v6GetXDot(v6), v6GetYDot(v6), v6GetZDot(v6)); return 0; }
int main(){ V6 v6; v6 = v6init(SPHERICAL); v6SetR(v6, 1e9); v6SetAlpha(v6, h2r(20.0)); v6SetDelta(v6, d2r(40.0)); v6 = v6s2c(v6); v6 = v6c2s(ellab(J2000, v6, -1)); printf("X %.9f \tY %.9f \tZ %.9f \nXDOT %.9f \tYDOT %.9f \tZDOT %.9f\n", v6GetX(v6), r2h(r2r(v6GetY(v6))), r2d(r2r(v6GetZ(v6))), v6GetXDot(v6), v6GetYDot(v6), v6GetZDot(v6)); return 0; }
int main(){ V6 v6; v6 = v6init(SPHERICAL); v6SetR(v6, 1.0); v6SetAlpha(v6, M_PI/4.0); v6SetDelta(v6, M_PI/4.0); v6 = v6s2c(v6); v6SetXDot(v6, -0.034); v6SetYDot(v6, -0.12); v6SetZDot(v6, -0.9); v6 = ecl2equ(v6, d2r(23.7)); printf("X %.9f \tY %.9f \tZ %.9f \nXDOT %.9f \tYDOT %.9f \tZDOT %.9f\n", v6GetX(v6), v6GetY(v6), v6GetZ(v6), v6GetXDot(v6), v6GetYDot(v6), v6GetZDot(v6)); return 0; }
void InitTweak( void ) { int rTweakValues[] = { 0, 5, 10, 30, 50, 70, 90, 110, 130, 150, 170 }; int count, rotate; double tweakRad; for( rotate = 0; rotate<2; rotate++ ) { for( count=0; count<=10; count++ ) { tweakRad = d2r( (90*rotate) - rTweakValues[count] ); tweakOffsetX[rotate][count] = (int) floor( 0.5 + cos( tweakRad ) * kBlobHorizSize ); tweakOffsetY[rotate][count] = (int) floor( 0.5 + sin( tweakRad ) * kBlobVertSize ); tweakOffsetX[rotate+2][count] = -tweakOffsetX[rotate][count]; tweakOffsetY[rotate+2][count] = -tweakOffsetY[rotate][count]; } } }