Esempio n. 1
0
// ==============================================================================================================================================
//
VECTOR3 SyncMFD::ComputeDeOrbit(double mu, double rea, VECTOR3 _entry, VECTOR3 _cp)
{
	double alpha  = angle(_entry, _cp);
	double refalt = length(_entry);
	double ca     = length(_cp);
	double vup    = sqrt(2.0*mu/refalt);	// Upper Limit = escape velocity
	double vlw    = 0.0;
	double v			= 0.0;
	double p			= 0.0;
	double a			= 0.0;
	double e			= 0.0;
	double t			= 0.0;
	double r			= 0.0;

	for (int i=0;i<32;i++) {

		v = (vup + vlw) / 2.0;

		p = cos(rea) * refalt * v; p*=p;	p/=mu;
		a = -mu*refalt / (v*v*refalt - 2.0*mu);
		e = sqrt(1.0 - p/a);
		t = PI2 - acos((p-refalt)/(refalt*e));
		r = p / (1.0+e*cos(t-alpha));

		if (r>ca) vup=v; else vlw=v;
	}

	VECTOR3 _n = crossp(_entry, _cp);
	VECTOR3 _q = crossp(_n, _cp);

	double fpa = tra2fpa(t-alpha, e);
	double ve  = sqrt(2.0*mu/ca - mu/a);
	VECTOR3 _v = unit(_q)*cos(fpa)*ve + unit(_cp)*sin(fpa)*ve;
	return _v;
}
Esempio n. 2
0
// ==============================================================================================================================================
// Define orbit by using ship's position vector and Periapsis vector 
//
bool Orbit::ApproachOrbit(OBJHANDLE ref, VECTOR3 pos, VECTOR3 peri)
{
	VECTOR3 normal = crossp(pos,peri);
	VECTOR3 minv   = crossp(normal,peri);

	double x,y;
	double tra = nangle(pos,peri,normal);
	double myy = oapiGetMass(ref) * GC;
	double ped = length(peri);
	double rad = length(pos);
	double ecc = ( ped - rad ) / ( rad*cos(tra)-ped );  // Eccentricity of orbit	
	double sma = ped/(1.0-ecc); // Semi-major axis
	double par = sma * (1.0-ecc*ecc); // Parameter
	double vel = sqrt(2.0*myy/rad - myy/sma); // Ship's velocity
	double smi = sqrt( fabs(sma) * par );
	double eca = tra2eca(tra,ecc);	

	if (ecc<1) { 
		x = -sma * sin(eca);
		y =  smi * cos(eca);
	} else {
		x = sma * sinh(eca);
		y = smi * cosh(eca);
	}
  
	double l = sqrt(x*x + y*y);

	VECTOR3 an = unit(peri) * (vel * x / l);
	VECTOR3 di = unit(minv) * (vel * y / l);
	
	Elements(pos,an+di,myy,false);

	return true;
}
Esempio n. 3
0
// ==============================================================================================================================================
//
void Orbit::CreateProjectionPlane(VECTOR3 normal, VECTOR3 zero)
{	
	Reset();

	myy = 1.327e20;
	ecc = 0;
	sma = AU;
	rad = AU;
	inc = 0;
	lan = 0;
	agp = 0;
	tra = 0;
	par = sma;
	mna = 0;
	mnm = MeanMotion();
	ang = sqrt(myy*par);
	lpe = 0;
	trl = 0;

	majv  = unit(crossp(crossp(normal,zero),normal));
	norv  = unit(normal);
	minv  = unit(crossp(norv,majv));
	Xmajv = majv * sma;
	Xminv = minv * sma;
	Xcv	  = _V(0,0,0);

	SetProjection(this);
}
Esempio n. 4
0
// ==============================================================================================================================================
//
void Orbit::CreateProjectionOrbit(VECTOR3 normal)
{	
	majv = unit(crossp(crossp(normal,zero),normal));
	norv = unit(normal);
	minv = unit(crossp(norv,majv));
	lan = 0.0;
	agp = 0.0;
	tra = 0.0;
	sma = AU;
	par = AU; 
	lpe = lan;
	trl = lan;
	Xmajv = majv * sma;
	Xminv = minv * sqrt(fabs(sma*par));
	Xcv	  = majv * (sma*ecc);
}
Esempio n. 5
0
// ==============================================================================================================================================
//
bool Orbit::DefineOrbit(OBJHANDLE ref, VECTOR3 pos, VECTOR3 normal,double tra,double ecc)
{

	VECTOR3 ma  = create_vector(normal,pos,PI2-tra);
	VECTOR3 mi  = unit(crossp(normal,ma));

	double x,y;
	double myy = oapiGetMass(ref) * GC;
	double rad = length(pos);
	double par = rad * (1.0+ecc*cos(tra));
	double sma = par / (1.0-ecc*ecc);
	double vel = sqrt(2.0*myy/rad - myy/sma); // Ship's velocity	
	double smi = sqrt( fabs(sma) * par );
	double eca = tra2eca(tra,ecc);	

	INVALIDD(par*sma*vel) return false;

	if (ecc<1.0) { 
		x = -sma * sin(eca);
		y =  smi * cos(eca);
	} else {
		x = sma * sinh(eca);
		y = smi * cosh(eca);
	}
  
	double l = sqrt(x*x + y*y);

	VECTOR3 an = ma * (vel * x / l);
	VECTOR3 di = mi * (vel * y / l);
	
	Elements(pos,an+di,myy,false);

	return true;
}
Esempio n. 6
0
void get_canvas_camera(dpoint3d &ipos, dpoint3d &istr, dpoint3d &ihei, dpoint3d &ifor)
{
	//ipo: camera position
	//ist: camera's unit RIGHT vector
	//ihe: camera's unit DOWN vector
	//ifo: camera's unit FORWARD vector

	ipos.x = 1024 -solid_view.m_lens_point[0];
	ipos.y = solid_view.m_lens_point[1];
	ipos.z = 256.0 - solid_view.m_lens_point[2];

	double f[3];
	for(int i = 0; i<3; i++)f[i] = solid_view.m_target_point[i] - solid_view.m_lens_point[i];
	norm(f);

	double down[3];
	for(int i = 0; i<3; i++)down[i] = -solid_view.m_vertical[i];

	double right[3];
	crossp(down, f, right);

	istr.x = -right[0];
	istr.y = right[1];
	istr.z = -right[2];

	ihei.x = -down[0];
	ihei.y = down[1];
	ihei.z = -down[2];

	ifor.x = -f[0];
	ifor.y = f[1];
	ifor.z = -f[2];
}
Esempio n. 7
0
// ==============================================================================================================================================
// Radius vector will point in vernal equinox
//
void Orbit::CreateNewCircularOrbit(Orbit *plane, double rad)
{
	double mu=plane->myy;
	double vcir=sqrt(mu/rad);
	VECTOR3 rv=plane->Position(0);
	VECTOR3 vv=crossp(plane->norv,rv);
	Elements(set_length(rv,rad),set_length(vv,vcir),mu,false);
}
Esempio n. 8
0
void CSolidView::LimitCamera(){
	if(m_lens_point[0] < 1)m_lens_point[0] = 1;
	if(m_lens_point[0] > 1023)m_lens_point[0] = 1023;
	if(m_lens_point[1] < 1)m_lens_point[1] = 1;
	if(m_lens_point[1] > 1023)m_lens_point[1] = 1023;
	if(m_lens_point[2] < 0)m_lens_point[2] = 0;
	if(m_lens_point[2] > 2048)m_lens_point[2] = 2048;

	// recalculate m_vertical
	double f[3];
	for(int i = 0; i<3; i++)f[i] = m_target_point[i] - m_lens_point[i];
	norm(f);
	double right[3];
	crossp(f, m_vertical, right);
	crossp(right, f, m_vertical);
	norm(m_vertical);
}
Esempio n. 9
0
void AscentAP::SetLaunchAzimuth (double azimuth)
{
	launch_azimuth = azimuth;

	// current launch location in local planet frame
	VECTOR3 pos, equ, dir, nml, ne, nd;
	double lng, lat, rad;
	double slng, clng, slat, clat;
	double saz = sin(azimuth), caz = cos(azimuth);
	OBJHANDLE hRef = vessel->GetGravityRef();
	vessel->GetGlobalPos(pos);
	oapiGlobalToLocal (hRef, &pos, &equ);
	oapiLocalToEqu (hRef, equ, &lng, &lat, &rad);
	slng = sin(lng), clng = cos(lng), slat = sin(lat), clat = cos(lat);
	normalise(equ); // unit radius vector
	
	// launch direction in local planet frame
	dir = _V(-clng*slat*caz - slng*saz, clat*caz, -slng*slat*caz + clng*saz);

	// normal of orbital plane in local planet frame
	nml = crossp(dir, equ);

	// normal of equator plane in local planet frame
	ne = _V(0,1,0);

	// direction of ascending node
	nd = unit (crossp(nml, ne));

	// orbit inclination
	tgt.inc = acos(dotp(nml, ne));

	// longitude of ascending node
	tgt.lan = atan2(nd.z, nd.x);

	// rotation matrix from equator plane to target orbit plane
	double sinc = sin(tgt.inc), cinc = cos(tgt.inc);
	double slan = sin(tgt.lan), clan = cos(tgt.lan);
	MATRIX3 R1 = _M(1,0,0, 0,cinc,sinc, 0,-sinc,cinc);
	MATRIX3 R2 = _M(clan,0,-slan, 0,1,0, slan,0,clan);
	tgt.R = mul(R2,R1);
}
Esempio n. 10
0
// ==============================================================================================================================================
//
double Orbit::TrlOfNode(VECTOR3 tgt_norv)
{
	if (Defined()) {
		VECTOR3 z; z.x=0; z.z=0; z.y=1.0;
		VECTOR3 vlan,vect;

		if (length(crossp(z,norv))==0) vlan=majv, vect=minv;
		else {
			vlan = unit(crossp(z,norv));
			vect = unit(crossp(norv,vlan));
		}

		VECTOR3 lv = crossp(norv, tgt_norv);
		
		double xf=dotp(lv,vect);                
		double xl=dotp(lv,vlan); 
			
		return limit(atan2(xf,xl)+lan);	
	}
	return 0;
}
Esempio n. 11
0
// ==============================================================================================================================================
//
void Orbit::GEO(OBJHANDLE ref)
{	
	if (!ref) {	GeoRef=NULL; return; }

	double obli  = oapiGetPlanetObliquity(ref);
	double peri  = oapiGetPlanetPeriod(ref);
	double trans = oapiGetPlanetTheta(ref);

	VECTOR3 rota, refd;
	PlanetAxis(obli,trans,&rota,&refd);

	VECTOR3 velo = crossp(rota,refd);
	double  myy  = oapiGetMass(ref)*GC;
	double  rad  = pow(fabs(peri)*sqrt(myy)/PI2, 2.0 / 3.0);
	double  vel  = sqrt(myy/rad);

	refd=set_length(refd,rad);
	velo=set_length(velo,vel);
	if (peri<0) velo=_V(0,0,0)-velo;

	Elements(refd,velo,myy);
	GeoRef=ref;
}
Esempio n. 12
0
const MATRIX3 &AttitudeReference::GetFrameRotMatrix () const
{
	// Returns rotation matrix for rotation from reference frame to global frame

	if (!valid_axes) {
		VECTOR3 axis1, axis2, axis3;
		switch (mode) {
		case 0:    // inertial (ecliptic)
			axis3 = _V(1,0,0);
			axis2 = _V(0,1,0);
			break;
		case 1: {  // inertial (equator)
			MATRIX3 R;
			oapiGetPlanetObliquityMatrix (v->GetGravityRef(), &R);
			//axis3 = _V(R.m13, R.m23, R.m33);
			axis3 = _V(R.m11, R.m21, R.m31);
			axis2 = _V(R.m12, R.m22, R.m32);
			} break;
		case 2: {  // orbital velocity / orbital momentum vector
			OBJHANDLE hRef = v->GetGravityRef();
			v->GetRelativeVel (hRef, axis3);
			axis3 = unit (axis3);
			VECTOR3 vv, vm;
			v->GetRelativePos (hRef, vv);    // local vertical
			vm = crossp (axis3,vv);    // direction of orbital momentum
			axis2 = unit (crossp (vm,axis3));
			} break;
		case 3: {  // local horizon / local north (surface)
			OBJHANDLE hRef = v->GetSurfaceRef();
			v->GetRelativePos (hRef, axis2);
			axis2 = unit (axis2);
			MATRIX3 prot;
			oapiGetRotationMatrix (hRef, &prot);
			VECTOR3 paxis = {prot.m12, prot.m22, prot.m32};  // planet rotation axis in global frame
			VECTOR3 yaxis = unit (crossp (paxis,axis2));      // direction of yaw=+90 pole in global frame
			axis3 = crossp (axis2,yaxis);
			} break;
		case 4: {  // synced to NAV source (type-specific)
			NAVDATA ndata;
			NAVHANDLE hNav = v->GetNavSource (navid);
			axis3 = _V(0,0,1);
			axis2 = _V(0,1,0);
			if (hNav) {
				oapiGetNavData (hNav, &ndata);
				switch (ndata.type) {
				case TRANSMITTER_IDS: {
					VECTOR3 pos, dir, rot;
					MATRIX3 R;
					VESSEL *vtgt = oapiGetVesselInterface (ndata.ids.hVessel);
					vtgt->GetRotationMatrix (R);
					vtgt->GetDockParams (ndata.ids.hDock, pos, dir, rot);
					axis3 = -mul(R,dir);
					axis2 = mul(R,rot);
					} break;
				case TRANSMITTER_VTOL:
				case TRANSMITTER_VOR: {
					OBJHANDLE hRef = v->GetSurfaceRef();
					VECTOR3 spos, npos;
					v->GetRelativePos (hRef, axis2);
					v->GetGlobalPos (spos);
					axis2 = unit (axis2);
					oapiGetNavPos (hNav, &npos);
					npos -= spos;
					axis3 = unit(crossp(crossp(axis2,npos),axis2));
					} break;
				}
			}
			} break;
		}
		axis1 = crossp(axis2,axis3);
		R = _M(axis1.x, axis2.x, axis3.x,  axis1.y, axis2.y, axis3.y,  axis1.z, axis2.z, axis3.z);

		valid_axes = true;
		valid_euler = false;
	}
	return R;
}
Esempio n. 13
0
bd(double* e, double dtheta, int flag, double* xin, double* xout)
/*
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

purpose

Subroutine bd performs the transformation between the coefficient vector and
the angular rate vector.

calling sequence

variable     i/o     description
--------     ---     -----------

e             i      unit vector along slew eigen-axis.

dtheta        i      slew angle (rad).

flag          i      flag determining direction of transformation.
                      = 0 -> compute coefficient vector from
                      angular rate vector
                      = 1 -> compute angular rate vector from
                      coefficient vector

xin           i      input vector.

xout          o      output vector.

return value

 0 -> no error
-1 -> transformation direction incorrectly specified.

external references

crossp

programming

J. J. McEnnan, April, 2003.

COPYRIGHT (C) 2003 by James McEnnan

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
{
  int i;
  double sa, ca, b0, b1, b2, temp1[3], temp2[3];

  if(dtheta > EPS)
  {
    ca = cos(dtheta);
    sa = sin(dtheta);

    if(flag == 0)
    {
      b1 = 0.5*dtheta*sa/(1.0 - ca);
      b2 = 0.5*dtheta;
    }
    else if(flag == 1)
    {
      b1 = sa/dtheta;
      b2 = (ca - 1.0)/dtheta;
    }
    else
      return -1;

    b0 = xin[0]*e[0] + xin[1]*e[1] + xin[2]*e[2];

    crossp(e,xin,temp2);

    crossp(temp2,e,temp1);

    for(i = 0;i < 3;i++)
      xout[i] = b0*e[i] + b1*temp1[i] + b2*temp2[i];
  }
  else
  {
    for(i = 0;i < 3;i++)
      xout[i] = xin[i];
  }

  return 0;
}
Esempio n. 14
0
rf(double* e, double dtheta, double* win, double* rhs)
/*
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

purpose

Subroutine rf computes the non-linear rate contributions to the final
angular acceleration.

calling sequence

variable     i/o     description
--------     ---     -----------

e             i      unit vector along slew eigen-axis.

dtheta        i      slew angle (rad).

win           i      input final angular rate vector.

rhs           o      output vector containing non-linear rate contributions
                     to the final acceleration.

return value

none

external references

crossp

programming

J. J. McEnnan, May, 2003.

COPYRIGHT (C) 2003 by James McEnnan

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
{
  int i;
  double sa, ca, dot, mag, c1, r0, r1, temp1[3], temp2[3];

  if(dtheta > EPS)
  {
    ca = cos(dtheta);
    sa = sin(dtheta);

    crossp(e,win,temp2);

    crossp(temp2,e,temp1);

    dot = win[0]*e[0] + win[1]*e[1] + win[2]*e[2];

    mag = win[0]*win[0] + win[1]*win[1] + win[2]*win[2];

    c1 = (1.0 - ca);

    r0 = 0.5*(mag - dot*dot)*(dtheta - sa)/c1;

    r1 = dot*(dtheta*sa - 2.0*c1)/(dtheta*c1);

    for(i = 0;i < 3;i++)
      rhs[i] = r0*e[i] + r1*temp1[i];
  }
  else
  {
    for(i = 0;i < 3;i++)
      rhs[i] = 0.0;
  }
}
Esempio n. 15
0
void slew3_init(double dt, double dtheta, double* e, double* wi, double* ai, double* wf, double* af)
/*
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

purpose

Subroutine slew3_init computes the coefficients for a third-order polynomial
interpolation function describing a slew between the input initial and
final states.

calling sequence

variable     i/o     description
--------     ---     -----------

dt            i      slew time (sec).

dtheta        i      slew angle (rad).

e             i      unit vector along slew eigen-axis.

wi            i      initial body angular rate (rad/sec).

ai            i      initial body angular acceleration (rad/sec^2)
                     (included for compatibility only).

wf            i      final body angular rate (rad/sec).

af            i      final body angular acceleration (rad/sec^2)
                     (included for compatibility only).

return value

none

external references

none

programming

J. J. McEnnan, March, 2003.

COPYRIGHT (C) 2003 by James McEnnan

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
{
  int i;
  double sa, ca, c1, c2;
  double b0, bvec1[3], bvec2[3], bvec[3];

  if(dt <= 0.0)
    return;

  sa = sin(dtheta);
  ca = cos(dtheta);

  /* final angular rate terms. */

  if(dtheta > EPS)
  {
    c1 = 0.5*sa*dtheta/(1.0 - ca);

    c2 = 0.5*dtheta;

    b0 = e[0]*wf[0] + e[1]*wf[1] + e[2]*wf[2];

    crossp(e,wf,bvec2);

    crossp(bvec2,e,bvec1);

    for(i = 0;i < 3;i++)
      bvec[i] = b0*e[i] + c1*bvec1[i] + c2*bvec2[i];
  }
  else
  {
    for(i = 0;i < 3;i++)
      bvec[i] = wf[i];
  }

  /* compute coefficients. */

  for(i = 0;i < 3;i++)
  {
    b[0][i] = wi[i];
    a[2][i] = e[i]*dtheta;
    b[2][i] = bvec[i];

    a[0][i] =  b[0][i]*dt;
    a[1][i] = (b[2][i]*dt - 3.0*a[2][i]);

    b[1][i] = (2.0*a[0][i] + 2.0*a[1][i])/dt;
    c[0][i] = (2.0*b[0][i] +     b[1][i])/dt;
    c[1][i] = (    b[1][i] + 2.0*b[2][i])/dt;

       d[i] = (    c[0][i] +     c[1][i])/dt;
  }

}
Esempio n. 16
0
// ==============================================================================================================================================
//
bool SyncMFD::InterpolateClosestPassage(OBJHANDLE ref, Orbit *src, double lon,double lat,double orbit,double *diff,double *tim,double *head,double *tr)
{
	VECTOR3 Rot,Off;
	Orbit LEO;
	int i;
	double obli = 0.0;
	double trans = 0.0;
	double per = 0.0;
	double offset = 0.0;
	double size = 0.0;
	double time = 0.0;

	// Rotation elements
	obli   = oapiGetPlanetObliquity(ref);
	trans  = oapiGetPlanetTheta(ref);
	per    = oapiGetPlanetPeriod(ref);
	offset = oapiGetPlanetCurrentRotation(ref);
	size   = oapiGetSize(ref);

	LEO.LEO(ref);

	double step  = PI2/16.01;
	double start = orbit;
	double end   = orbit+PI2+(step/2.0);
	double trl, dst2;
	double dot,old_dot=0;


	VECTOR3 pos = _V(0,0,0);
	VECTOR3 vel = _V(0,0,0);
	VECTOR3 gpv = _V(0,0,0);
	VECTOR3 spd = _V(0,0,0);
	VECTOR3 relv = _V(0,0,0);
	VECTOR3 relp = _V(0,0,0);

	bool no_intercept=true;
	bool first=true;

	for (trl=start;trl<end;trl+=step) {

		// Caluclate time to a location
		time=Trl2Time(trl,src)/86400.0;

		// Caluclate Planets Rotation offset at that time
		PlanetAxis(obli,trans,offset,per,time,&Rot,&Off);

		// Calculate Ship's position vector and distance^2
		pos  = src->Position(trl);
		dst2 = dotp(pos,pos);

		// Reset the distance to correspond a surface position.
		pos  = set_length(pos,size);

		// Compute ship's surface velocity vector direction
		vel = crossp(src->norv,pos);

		// Compute ship's surface velocity vector magnitude
		vel = set_length(vel,src->ang*size/dst2);

		// Compute base's position vector
		gpv = VectorByLonLat(Rot,Off,lon,lat)*size;

		// Compute speed of the base
		spd = GroundSpeedVector(ref,time,lon,lat);

		// Copmute relative position to the base
		relv = (vel-spd);
		relp = (pos-gpv);

		dot = dotp(relv,relp);

		if (old_dot<0 && dot>0 && !first) {
			if (angle(pos,gpv)<PI05) {
				start=trl-step;
				no_intercept=false;
				break;
			}
		}

		first=false;
		old_dot=dot;
	}


    // Return N/A if no passage points found
	if (no_intercept) {
		if (head) *head = 0;
		if (diff) *diff = 0;
		if (tim)  *tim  = 0;
		if (tr)   *tr   = start+PI2-PI05;
		return false;
	}


	trl=start;
	old_dot=-1; // We are approaching the base
	double trll=0;
	no_intercept=true;

	for (i=0;i<24;i++) {

		step/=2.0;

		// Caluclate time to a location
		time=Trl2Time(trl,src)/86400.0;

		// Caluclate Planets Rotation offset at that time
		PlanetAxis(obli,trans,offset,per,time,&Rot,&Off);

		// Calculate Ship's position vector and distance^2
		pos  = src->Position(trl);
		dst2 = dotp(pos,pos);

		// Reset the distance to correspond a surface position.
		pos  = set_length(pos,size);

		// Compute ship's surface velocity vector direction
		vel = crossp(src->norv,pos);

		// Compute ship's surface velocity vector magnitude
		vel = set_length(vel,src->ang*size/dst2);

		// Compute base's position vector
		gpv = VectorByLonLat(Rot,Off,lon,lat)*size;

		// Compute speed of the base
		spd = GroundSpeedVector(ref,time,lon,lat);

		// Copmute relative position to the base
		relv = (vel-spd);
		relp = (pos-gpv);

		dot = dotp(relv,relp);

		trll=trl;
		if (dot==0) {
			 no_intercept=false;
			 break; // closest position archived, break
		}

		if (dot>0) { // base is behind of us, step back
			trl=trl-step;
			no_intercept=false;  // Verify, We have a base passage.
		}
		else trl=trl+step;	// base is front of us, cuntinue stepping
	}


	double dif=angle(pos, gpv);

	VECTOR3 zero=crossp(pos,spd);

	if (head) *head = nangle(relp,zero,pos);
	if (diff) *diff = dif;
	if (tim)  *tim  = time*86400.0;
	if (tr)   *tr   = trll;

	return true;
}
Esempio n. 17
0
// ==============================================================================================================================================
//
void SyncMFD::Update(HDC hDC)
{

	int pos;
	int ld=MFD->LineHeight;
	int fbp=MFD->FirstButton;
	char *Mnu = {"BaseSync v3.2\0"};

	int width=MFD->width;
	int height=MFD->height;

	double w=(double) width;
	double h=(double) height;

	int i;
	VECTOR3 Rot,Off,opos;
	double obli,trans,per,offset,diff,p,op,t,time,lon,lat;
	double trl,heading;
	char   name[32];
	static const int MAXSOLN = 160;
	double times[MAXSOLN],diffs[MAXSOLN],heads[MAXSOLN];
	int sol_found = 0;
	double max_diff=1000;
	bool   draw_text=false;

	Orbit LEO,ShipOrbit,Ecliptic;

	SetTextColor(hDC,white);
	SetTextAlign(hDC,TA_LEFT);
	TextOut(hDC,5,1,Mnu, (int)strlen(Mnu));

	if (mode.enc==0) Text(hDC,5,1+ld,"Latitude");
	if (mode.enc==1) Text(hDC,5,1+ld,"Closest passage");
	if (mode.enc==2) Text(hDC,5,1+ld,"Apoapsis");
	if (mode.enc==3) Text(hDC,5,1+ld,"Periapsis");

	if (sync_num<1) sync_num=1;
	if (sync_num>99) sync_num=99;


	OBJHANDLE ref = oapiGetObjectByName(trgt->ref);
	if (ref==NULL) {
    usingGS2 = false;
    trgt = &bstrgt;
    ref=ship->GetSurfaceRef();
  }

	if (ref==NULL) return;



	ShipOrbit.Elements(ship->GetHandle(), ref);
	Ecliptic.Ecliptic();

	oapiGetObjectName(ref, trgt->ref, 31);


  // Rotation elements

	obli   = oapiGetPlanetObliquity(ref);
	trans  = oapiGetPlanetTheta(ref);
	per    = oapiGetPlanetPeriod(ref);
	offset = oapiGetPlanetCurrentRotation(ref);

	// LEO
	LEO.LEO(ref);

  if (!usingGS2) {
    OBJHANDLE tgt = oapiGetBaseByName(ref, trgt->name);
    if (tgt) {
		  oapiGetObjectName(tgt,trgt->name,32);
		  oapiGetBaseEquPos(tgt,&trgt->lon, &trgt->lat);
	  }
	  else strcpy(trgt->name,"Surface");
  }


	double trle=ShipOrbit.TrlOfNode(&LEO);
	double EqI=angle(ShipOrbit.norv, LEO.norv);
	double ang=asin(sin(trgt->lat) / sin(EqI));

	double apos=limit(trle+ang);
	double bpos=limit(trle+PI-ang);

	double atime=ShipOrbit.TimeTo(apos);
	double btime=ShipOrbit.TimeTo(bpos);

	double dist=ShipOrbit.PeriapsisDistance();
	double apodist = dist=ShipOrbit.AopapsisDistance();
	if (dist<oapiGetSize(ref)) dist=oapiGetSize(ref);

	double zoom=w/(2.55 * (apodist + apodist + dist) / 3.0);
	double r=oapiGetSize(ref)*zoom;

	double x=w/2,y=h/2;
	double intpos=0;

	if (display_texts&2) {
		SelectObject(hDC,solid_pen_dgrey);
		DrawEllipse(hDC,x-r,y-r,x+r,y+r,w,h);

		if (mode.enc==0) {  // Latitude
			if ((sync_sel&1)==0) SelectObject(hDC,solid_pen_y), intpos=apos;
			else SelectObject(hDC,solid_pen_dgrey);
			r=ShipOrbit.Radius(apos)*zoom;
			DrawLine(hDC,x,y,x+r*cos(apos),x-r*sin(apos),w,h,false);

			if ((sync_sel&1)==1) SelectObject(hDC,solid_pen_y), intpos=bpos;
			else SelectObject(hDC,solid_pen_dgrey);
			r=ShipOrbit.Radius(bpos)*zoom;
			DrawLine(hDC,x,y,x+r*cos(bpos),x-r*sin(bpos),w,h,false);
		}

		if (mode.enc==1) { // Closest Passage
			r=ShipOrbit.Radius(sync_trl)*zoom;
			intpos=sync_trl;

			SelectObject(hDC,solid_pen_y);
			DrawLine(hDC,x,y,x+r*cos(sync_trl),x-r*sin(sync_trl),w,h,false);
		}

		if (mode.enc==2 || mode.enc==3) { // Aopapsis Periapsis
			SelectObject(hDC,solid_pen_y);
			r=ShipOrbit.Radius(sync_trl)*zoom;
			DrawLine(hDC,x,y,x+r*cos(sync_trl),x-r*sin(sync_trl),w,h,false);

			SelectObject(hDC,solid_pen_grey);
			r=ShipOrbit.Radius(sync_line)*zoom;
			DrawLine(hDC,x,y,x+r*cos(sync_line),x-r*sin(sync_line),w,h,false);
		}

		ShipOrbit.SetProjection(&ShipOrbit);
		ShipOrbit.GDIDraw(hDC,green,w,h,zoom,true,true);

		if (mode.deo) {
			r=ShipOrbit.Radius(deo.trlBurn)*zoom;
			SelectObject(hDC,solid_pen_white);
			DrawLine(hDC,x,y,x+r*cos(deo.trlBurn),x-r*sin(deo.trlBurn),w,h,false);
		}
	}



	if (EqI>=trgt->lat || mode.enc!=0) {
		// Usual case... target in range

		draw_text=true;
		SetTextColor(hDC,green);
		pos=(fbp+(ld*8));

		if (display_texts&1 && !mode.deo) {
			if (mode.enc==0) {
				Text(hDC,5,pos," #: Time:");
			Text(hDC,width/2,pos,"Lon Diff:"), pos+=ld;
			}	else {
				Text(hDC,5,pos," #: Time:");
			Text(hDC,5+width/3,pos,"  Dist:");
			Text(hDC,5+width*2/3,pos,"  Heading:"), pos+=ld;
			}
		}

		if (atime<btime && atime>0) sync_sel=0;
		else if (btime>0) sync_sel=1;
		else sync_sel=0;

		for (i=0;i<MAXSOLN;i++) {
			times[i]=0;
			diffs[i]=0;
		}
		sync_min = -1;


		if (ShipOrbit.ecc<1 && mode.enc==0) {
			for (i=0;i<MAXSOLN;i++) {

				if (i&1) time=MAX(atime, btime);
				else     time=MIN(atime, btime);

				if (time==atime) op=apos;
				else             op=bpos;

				p=(double)i;
				if (i>1) time+=ShipOrbit.Period()*floor(p/2);

				t = time/86400;
				PlanetAxis(obli,trans,offset,per,t,&Rot,&Off);

				opos=ShipOrbit.Position(op);

				LonLat(opos,Rot,Off,&lon,&lat);
				diff=lon-trgt->lon;

				if (fabs(diff)<max_diff) {
					max_diff=fabs(diff), sync_time=time, sync_trl=op;
					sync_min=i;
				}

				times[i]=time;
				diffs[i]=diff;
				if (time > 0.0 && diff >0.0) {
					sol_found++;
					if (sol_found==sync_num) break;
				}
			}
		}


		max_diff=1e10;

		if (ShipOrbit.ecc<1 && mode.enc==1) {

			double posit=ShipOrbit.trl;

			for (i=0;i<MAXSOLN;i++) {

				InterpolateClosestPassage(ref,&ShipOrbit,trgt->lon,trgt->lat,posit,&diff,&time,&heading,&trl);

				posit=trl+PI05;

				if (diff<max_diff && diff>0) {
					max_diff=diff, sync_time=time, sync_trl=limit(trl);
					sync_min=i;
				}

				heads[i]=heading;
				times[i]=time;
				diffs[i]=diff;
				if (time > 0.0 && diff >0.0) {
					sol_found++;
					if (sol_found==sync_num) break;
				}
			}
		}

		if (ShipOrbit.ecc<1 && (mode.enc==2 || mode.enc==3)) {
			for (i=0;i<MAXSOLN;i++) {

				if (mode.enc==2) sync_line=limit(ShipOrbit.lpe+PI);
				else sync_line=limit(ShipOrbit.lpe);

				time=ShipOrbit.TimeTo(sync_line) + ShipOrbit.Period() * (double)i;
				t = time / 86400.0;

				PlanetAxis(obli,trans,offset,per,t,&Rot,&Off);

				VECTOR3 gpv=VectorByLonLat(Rot,Off,trgt->lon,trgt->lat);
							VECTOR3 pos=ShipOrbit.Position(sync_line);

				diff = angle(gpv,pos);
				heading = nangle(pos-gpv,Rot,gpv);

				ShipOrbit.Longitude(gpv,NULL,NULL,&trl);

				if (diff<max_diff && diff>0) {
					max_diff=diff, sync_time=time, sync_trl=limit(trl);
					sync_min=i;
				}

				if (time==0) time=0.1;
				heads[i]=heading;
				times[i]=time;
				diffs[i]=diff;
				if (time > 0.0 && diff >0.0) {
					sol_found++;
					if (sol_found==sync_num) break;
				}
			}
		}

		// Hyperbolic Orbit
		if (ShipOrbit.ecc>=1 && (atime>0 || btime>0)) {
			mode.enc=0;
			for (i=0;i<2;i++) {

				if (atime>0 && btime>0) {
					if (i&1) time=MAX(atime, btime);
					else     time=MIN(atime, btime);
				}
				else time=(atime>0 ? atime : btime);

				op = (time==atime? apos : bpos);

				p=(double)i;
				if (i>1) time+=ShipOrbit.Period()*floor(p/2);
				t = time/86400;
				PlanetAxis(obli,trans,offset,per,t,&Rot,&Off);

				opos=ShipOrbit.Position(op);
				LonLat(opos,Rot,Off,&lon,&lat);
				diff=lon-trgt->lon;

				if (fabs(diff)<max_diff) {
					max_diff=fabs(diff), sync_time=time, sync_trl=op;
					sync_min=i;
				}

				times[i]=time;
				diffs[i]=diff;

				if (atime<0 || btime<0) break;
			}
		}

		if (sync_min > -1) {
			sol.num = sync_min+1;
			sol.tSol = times[sync_min];
			sol.dist = diffs[sync_min];
			sol.hdg = heads[sync_min];
			sol.dataValid = true;
		} else {
			sol.dataValid = false;
		}

		double rad=oapiGetSize(ref);
		int no=0;
		int disp_i = 1;

		if (display_texts&1 && !mode.deo) {
			for (i=0;i<MAXSOLN;i++) {
				if (i==sync_min) SetTextColor(hDC,lyellow);
				else SetTextColor(hDC,lgreen);
				if (times[i]>0.0 && diffs[i]>=0.0) {
					if (disp_i >= sync_dispmin && disp_i <= sync_dispmin+7) {
						if (mode.enc==0) {
							sprintf(name,"%2d: ",disp_i);
							Text(hDC,5,pos,name,times[i]);
							TextA(hDC,width/2,pos,"",diffs[i]*DEG), pos+=ld;
						}	else { // enc_mode 1,2,3
							no++;
							sprintf(name,"%2d: ",disp_i);
							Text(hDC,5,pos,name,times[i]);
							Text(hDC,5+width/3,pos,"  ",diffs[i]*rad);
							TextA(hDC,5+width*2/3,pos,"  ",heads[i]*DEG), pos+=ld;
						}
					}
					disp_i++;
					if (disp_i > sync_num) break;
				}
			}
		}
		sync_sel+=sync_min;
	}	else {
		// We are in Latitude mode, and the target is outside of our inclination - i.e. no solution

		sol.dataValid = false;
		SetTextAlign(hDC,TA_CENTER);
		SetTextColor(hDC,yellow);
		pos=(fbp+(ld*8));
		Text(hDC,width/2,pos,"Target Out of Range"); pos+=ld;
	}



	if (mode.dir) {
		// Calculate burn for plane change correction in DIRECT mode

		if (EqI<trgt->lat && mode.enc==0) {
			double trl;
			PlanetAxis(obli,trans,offset,per,0,&Rot,&Off);
			VECTOR3 gpv = VectorByLonLat(Rot,Off,trgt->lon,trgt->lat);
			ShipOrbit.Longitude(gpv,NULL,NULL,&trl);
			sync_time=ShipOrbit.TimeTo(trl);
			sync_trl=trl;
		}

		time_to_int=sync_time;
		double trl=sync_trl;

		PlanetAxis(obli,trans,offset,per,time_to_int/86400.0,&Rot,&Off);
		VECTOR3 pos = ShipOrbit.Position(trl);
		VECTOR3 gpv = VectorByLonLat(Rot,Off,trgt->lon,trgt->lat);
		VECTOR3 lan = crossp(gpv, pos);
		VECTOR3 nor = ShipOrbit.norv;

		ShipOrbit.Longitude(lan,NULL,NULL,&sol.trlBurn);

		sol.rIn = fabs(asin(dotp(gpv,nor)));

		double a = ShipOrbit.TimeToPoint(sol.trlBurn);
		double b = ShipOrbit.TimeToPoint(limit(sol.trlBurn+PI));

		if (fabs(a)<fabs(b)) sol.tToBurn =a, sol.nmlBurn=true;
		else				 sol.tToBurn =b, sol.nmlBurn=false;

		sol.dV = sol.rIn*ShipOrbit.ang/ShipOrbit.Radius(sol.trlBurn);
		sol.tBurn =BurnTimeBydV(sol.dV,ship);

	}	else {
		// Calculate burn for plane change correction in EQUATORIAL mode

		sol.trlBurn=ShipOrbit.TrlOfNode(&LEO);
		sol.rIn=MAX(trgt->lat-EqI,0);

		double a=ShipOrbit.TimeToPoint(sol.trlBurn);
		double b=ShipOrbit.TimeToPoint(limit(sol.trlBurn+PI));

		if (fabs(a)<fabs(b)) sol.tToBurn =a, sol.nmlBurn=true;
		else				 sol.tToBurn =b, sol.nmlBurn=false;

		sol.dV = sol.rIn*ShipOrbit.ang/ShipOrbit.Radius(sol.trlBurn);
		sol.tBurn =BurnTimeBydV(sol.dV,ship);
	}

	if (display_texts&2) {
		ShipOrbit.DrawPlaneIntersection(hDC,sol.trlBurn,w/2,h/2,zoom,grey);
	}

	SetTextAlign(hDC,TA_LEFT);
	pos=1;
	SetTextColor(hDC,grey);
  if (usingGS2) {
	  Text(hDC,width*1/2,pos,"Linked ","to GS"); pos+=ld;

    // Check if GS2 changed target
    if (bstrgt.lat != gs2trgt->lat || bstrgt.lon != gs2trgt->lon) {
        strcpy(bstrgt.ref, gs2trgt->ref);
        strcpy(bstrgt.name, gs2trgt->name);
        bstrgt.lat = gs2trgt->lat;
        bstrgt.lon = gs2trgt->lon;
        bstrgt.alt = gs2trgt->alt;
        bstrgt.ang = gs2trgt->ang;
        bstrgt.ant = gs2trgt->ant;
    }

  } else {
	  Text(hDC,width*1/2,pos,"Ref ",trgt->ref); pos+=ld;
  }
  Text(hDC,width*1/2,pos,"Tgt ",trgt->name); pos+=ld;

	if (display_texts&1) {
		if (!mode.deo) {
			// Display the burn solution for plane change

			deo.dataValid = false;
			pos=fbp;
			SetTextColor(hDC,lgreen);

			if (trgt->lat<0) TextA(hDC,5,pos,"Lat ",fabs(DEG*trgt->lat),"S"), pos+=ld;
			else TextA(hDC,5,pos,"Lat ",DEG*trgt->lat,"N"), pos+=ld;

			if (trgt->lon<0) TextA(hDC,5,pos,"Lon ",fabs(DEG*trgt->lon),"W"), pos+=ld;
			else TextA(hDC,5,pos,"Lon ",DEG*trgt->lon,"E"), pos+=ld;

			TextA(hDC,5,pos,"EqI ",EqI*DEG), pos+=ld;


			PlanetAxis(ref,0,&Rot,&Off);
			VECTOR3 base_pos = VectorByLonLat(Rot,Off,trgt->lon,trgt->lat);

			VECTOR3 ship_pos; ship->GetRelativePos(ref,ship_pos);
			VECTOR3 ship_vel; ship->GetRelativeVel(ref,ship_vel);

			VECTOR3 gsp=GroundSpeedVector(ref,ship);
			VECTOR3 zero=crossp(ship_pos,ship_vel);
			double head = nangle(gsp,zero,ship_pos);

      double baseHeight = oapiGetSize(ref);
#if ORBITER_VERSION == 2016
      ELEVHANDLE eh = oapiElevationManager(ref);
      if (eh) {
        baseHeight += oapiSurfaceElevation(ref, trgt->lon, trgt->lat);
      }
#endif


			TextA(hDC,5,pos,"Hed ",head*DEG), pos+=ld;
			Text(hDC,5,pos, "GSp ",length(gsp)), pos+=ld;
			Text(hDC,5,pos, "Dst ",angle(ship_pos,base_pos)*baseHeight), pos+=ld;
			if (sol.dataValid) {
        double altitude = length(ShipOrbit.Position(intpos));
        altitude -= baseHeight;
				Text(hDC,5,pos, "Alt ",altitude);
      }
			pos+=ld;


			pos=fbp;
			int xx=width/2;
			SetTextColor(hDC,green);
			if (mode.dir) Text(hDC,xx,pos,"Direct:"), pos+=ld;
			else Text(hDC,xx,pos,"Equator:"), pos+=ld;

			SetTextColor(hDC,lgreen);
			TextA(hDC,xx,pos,"RIn ",sol.rIn*DEG), pos+=ld;
			TextA(hDC,xx,pos,"LAN ",sol.trlBurn*DEG), pos+=ld;
			Text(hDC,xx,pos," Tn ",sol.tToBurn), pos+=ld;
      if (abs(sol.tBurn) > 0.1) {
  			if (sol.nmlBurn) Text(hDC,xx,pos,"PlC ",sol.tBurn,"s (+)"), pos+=ld;
	  		else Text(hDC,xx,pos,"PlC ",sol.tBurn,"s (-)"), pos+=ld;
		  	Text(hDC,xx,pos," dV ",sol.dV,"m/s"), pos+=ld;
      } else {
        Text(hDC,xx,pos,"PlC 0.000s"), pos+=ld;
		  	Text(hDC,xx,pos," dV 0.000m/s"), pos+=ld;
      }

      if (sol.dataValid) {
        burn.dV = sol.dV;
        burn.orientation = sol.nmlBurn ? 1 : -1;
        burn.tToInstBurn = sol.tToBurn;
        burn.dataValid = true;
      } else {
        burn.dataValid = false;
      }

		}	else {
			// Display the deorbit parameters and burn solution

			pos=fbp;
			SetTextColor(hDC,white);
			Text(hDC,5,pos,"De-Orbit Program"), pos+=2*ld;

			SetTextColor(hDC,lgreen);
      if (usingGS2) {
			  Text(hDC,5,pos, "Ang, Ant, Alt from GS"), pos+=ld;
        TextA(hDC,5,pos,"GS Ang ",trgt->ang*DEG), pos+=ld;
			  TextA(hDC,5,pos,"GS Ant ",trgt->ant*DEG), pos+=ld;
			  Text(hDC,5,pos, "GS Alt ",trgt->alt,"km"), pos+=2*ld;
      } else {
        TextA(hDC,5,pos,"Ang ",bstrgt.ang*DEG), pos+=ld;
			  TextA(hDC,5,pos,"Ant ",bstrgt.ant*DEG), pos+=ld;
			  Text(hDC,5,pos, "Alt ",bstrgt.alt,"km"), pos+=2*ld;
      }
			double pre;
			ComputeDeOrbit(ShipOrbit.myy,ShipOrbit.rad,trgt->alt*1000+oapiGetSize(ref),trgt->ang,&pre,&deo.dV);
			deo.dV=ShipOrbit.vel-deo.dV;

			deo.trlBurn = limit(sync_trl - trgt->ant - pre);
			deo.tInstBurn = ShipOrbit.TimeToPoint(deo.trlBurn);
			deo.tBurn = BurnTimeBydV(deo.dV,ship);
			deo.tToBurn = deo.tInstBurn - 0.5 * deo.tBurn;

			Text(hDC,5,pos, "TBn ",deo.tToBurn,"s"), pos+=ld;
			TextA(hDC,5,pos,"TrL ",deo.trlBurn*DEG), pos+=ld;
			Text(hDC,5,pos, " dV ",deo.dV,"m/s"), pos+=ld;
			Text(hDC,5,pos, " BT ",deo.tBurn,"s"), pos+=2*ld;

			if (ShipOrbit.ecc>0.015) {
				SetTextColor(hDC,lgreen);
				Text(hDC,5,pos,"De-orbit burn data only accurate"); pos+=ld;
				Text(hDC,5,pos,"if your Ecc is 0.015 or less."); pos+=ld;
				Text(hDC,5,pos,"(If burn complete, please ignore.)"); pos+=ld;pos+=ld;
			}
			if (!mode.dir) {
				SetTextColor(hDC,lgreen);
				Text(hDC,5,pos,"Use \"Direct\" mode to re-synchronize"); pos+=ld;
				Text(hDC,5,pos,"the approach after the de-orbit");
			}
			deo.dataValid = true;
      burn.dV = -deo.dV;
      burn.orientation = 0;
      burn.tToInstBurn = deo.tToBurn;
      burn.dataValid = true;
		}
	}

	if (!MMPut_done) {
    mma.PutMMStruct("BaseSyncTarget", &bstrgt);
    mma.PutMMStruct("BaseSyncMode", &mode);
    mma.PutMMStruct("BaseSyncSolution", &sol);
    mma.PutMMStruct("BaseSyncDeorbit", &deo);
    mma.PutMMStruct("BaseSyncBurn", &burn); // To be removed after BTC synchronization
		if (burn.dataValid)
        {
            mma.Put("dv", burn.dV);
            mma.Put("InstantaneousBurnTime", burn.tToInstBurn);
            mma.Put("Orientation", burn.orientation);
        }
        else
        {
          mma.Delete("dv");
          mma.Delete("InstantaneousBurnTime");
          mma.Delete("Orientation");
        }
		MMPut_done = true;
	}
}
Esempio n. 18
0
// ==============================================================================================================================================
//
void Orbit::Elements(VECTOR3 r, VECTOR3 v,double m,bool reset_proj)
{	
	Reset();

	if (fabs(r.x)<0.01) r.x=0;	if (fabs(r.y)<0.01) r.y=0;
	if (fabs(r.z)<0.01) r.z=0;	if (fabs(v.x)<0.01) v.x=0;
	if (fabs(v.y)<0.01) v.y=0;	if (fabs(v.z)<0.01) v.z=0;

	vv=v; rv=r;

	double vel2;
	VECTOR3 z; z.x=0; z.z=0; z.y=1.0;
   
	vel2 = dotp(v,v);
	vel  = sqrt(vel2);
	rad  = length(r);
	myy  = m;

	// Computer normal vector and vector pointing ascenting node
	norv	= crossp(r,v);
	ang		= length(norv);
	norv	= norv*(1.0/ang);

	VECTOR3 ANv = unit(crossp(z,norv));
    
	// Inclination
	inc = acos(-norv.y);
	par = ang*ang/myy; 
	majv =( r * (vel2-myy/rad) ) - (v * dotp(r,v));

	double ml=length(majv);

	ecc  = ml/myy;
	
	if (ecc<1e-6) ecc=0;
	if (inc<1e-6) inc=0;

	sma = par / (1.0-ecc*ecc);

	r=r*(1.0/rad);					// Make the radius vector to unit size, After computing ecc-vector

	if (inc==0) ANv=_V(1,0,0);		// Place ANv to vernal equinox
	if (ecc!=0) majv=majv*(1.0/ml); // Make the major vector to unit size	
	else majv=ANv;					// Place major vector to ascenting node
	
	// Longitude of ascenting node
	if (inc!=0) {
		double  x=ANv.x;
		if (x>=1) {
			lan=0;
		} else if (x<=-1) {
			lan=PI;
		} else {		
			lan=acos(x);		
			if (ANv.z<0) lan=PI2-lan;		
		}
	} else {
		lan=0.0;
	}
	
	// Argument of Periapsis
	if (inc!=0 && ecc!=0) {
		double x=dotp(ANv,majv);
		if (x>1) x=1; // Avoid some precision problems
		else if (x<-1) x=-1;
		agp=acos(x);
		if (majv.y<0) agp=PI2-agp;
	} else if (ecc!=0) {
		agp=acos(majv.x);
		if (majv.z<0) agp=PI2-agp;
	}
	else agp=0.0;

  // True anomaly
	if (ecc!=0) {
		double x=dotp(majv,r);
		if      (x>=1)  tra=0;
		else if (x<=-1) tra=PI;
		else {
			tra=acos(x);
			x=dotp(r,v);
			if (fabs(x)<1e-6) x=0; // Avoid some precision problems
			if (x<0) tra=PI2-tra;
		}
	} else if (inc!=0) {
		tra=acos(dotp(ANv,r));
		if (dotp(ANv,v)>0) tra=PI2-tra;
	} else {
		tra=acos(r.x);
		if (v.x>0) tra=PI2-tra;
	}

	lpe=limit(agp+lan);				// Longitude of Periapsis
	trl=limit(lpe+tra);				// True longitude
	minv=unit(crossp(norv,majv));	// Minor axis vector
	mna=tra2mna(tra,ecc);			// Mean anomaly
	mnm=MeanMotion();

	Xmajv = majv * sma;
	Xminv = minv * sqrt(fabs(sma*par));
	Xcv	  = majv * (sma*ecc);

	if (reset_proj) SetProjection(this);
}
Esempio n. 19
0
void slew3(double t, double dt, double* qi, double* q, double* omega, double* alpha, double* jerk)
/*
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

purpose

Subroutine slew3 computes the quaternion, body angular rate, acceleration and
jerk as a function of time corresponding to a third-order polynomial
interpolation function describing a slew between initial and final states.

calling sequence

variable     i/o     description
--------     ---     -----------

t             i      current time (seconds from start).

dt            i      slew time (sec).

qi            i      initial attitude quaternion.

q             o      current attitude quaternion.

omega         o      current body angular rate (rad/sec).

alpha         o      current body angular acceleration (rad/sec^2).

jerk          o      current body angular jerk (rad/sec^3).

return value

none

external references

unvec

programming

J. J. McEnnan, March, 2003.

COPYRIGHT (C) 2003 by James McEnnan

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
{
  int i;
  double x, ang, sa, ca, u[3], x1[2], unvec();
  double th0[3], th1[3], th2[3], th3[3], temp0[3], temp1[3], temp2[3];
  double thd1, thd2, thd3, w2, td2, ut2, wwd;
  double w[3], udot[3], wd1[3], wd1xu[3], wd2[3], wd2xu[3];

  if(dt <= 0.0)
    return;

  x = t/dt;

  x1[0] = x - 1.0;

  for(i = 1;i < 2;i++)
    x1[i] = x1[i - 1]*x1[0];

  for(i = 0;i < 3;i++)
  {
    th0[i] = ((x*a[2][i] + x1[0]*a[1][i])*x + x1[1]*a[0][i])*x;

    th1[i] = (x*b[2][i] + x1[0]*b[1][i])*x + x1[1]*b[0][i];

    th2[i] = x*c[1][i] + x1[0]*c[0][i];

    th3[i] = d[i];
  }

  ang = unvec(th0,u);

  ca = cos(0.5*ang);
  sa = sin(0.5*ang);

  q[0] = ca*qi[0] + sa*( u[2]*qi[1] - u[1]*qi[2] + u[0]*qi[3]);
  q[1] = ca*qi[1] + sa*(-u[2]*qi[0] + u[0]*qi[2] + u[1]*qi[3]);
  q[2] = ca*qi[2] + sa*( u[1]*qi[0] - u[0]*qi[1] + u[2]*qi[3]);
  q[3] = ca*qi[3] + sa*(-u[0]*qi[0] - u[1]*qi[1] - u[2]*qi[2]);

  ca = cos(ang);
  sa = sin(ang);

  if(ang > EPS)
  {
    /* compute angular rate vector. */

    crossp(u,th1,temp1);

    for(i = 0;i < 3;i++)
      w[i] = temp1[i]/ang;

    crossp(w,u,udot);

    thd1 = u[0]*th1[0] + u[1]*th1[1] + u[2]*th1[2];

    for(i = 0;i < 3;i++)
      omega[i] = thd1*u[i] + sa*udot[i] - (1.0 - ca)*w[i];

    /* compute angular acceleration vector. */

    thd2 = udot[0]*th1[0] + udot[1]*th1[1] + udot[2]*th1[2] +
              u[0]*th2[0] +    u[1]*th2[1] +    u[2]*th2[2];

    crossp(u,th2,temp1);

    for(i = 0;i < 3;i++)
      wd1[i] = (temp1[i] - 2.0*thd1*w[i])/ang;

    crossp(wd1,u,wd1xu);

    for(i = 0;i < 3;i++)
      temp0[i] = thd1*u[i] - w[i];

    crossp(omega,temp0,temp1);

    for(i = 0;i < 3;i++)
      alpha[i] = thd2*u[i] + sa*wd1xu[i] - (1.0 - ca)*wd1[i] +
      thd1*udot[i] + temp1[i];

    /* compute angular jerk vector. */

    w2 = w[0]*w[0] + w[1]*w[1] + w[2]*w[2];

    thd3 = wd1xu[0]*th1[0] + wd1xu[1]*th1[1] + wd1xu[2]*th1[2] -
           w2*(u[0]*th1[0] + u[1]*th1[1] + u[2]*th1[2]) +
           2.0*(udot[0]*th2[0] + udot[1]*th2[1] + udot[2]*th2[2]) +
           u[0]*th3[0] + u[1]*th3[1] + u[2]*th3[2];

    crossp(th1,th2,temp1);

    for(i = 0;i < 3;i++)
      temp1[i] /= ang;

    crossp(u,th3,temp2);

    td2 = (th1[0]*th1[0] + th1[1]*th1[1] + th1[2]*th1[2])/ang;

    ut2 = u[0]*th2[0] + u[1]*th2[1] + u[2]*th2[2];

    wwd = w[0]*wd1[0] + w[1]*wd1[1] + w[2]*wd1[2];

    for(i = 0;i < 3;i++)
      wd2[i] = (temp1[i] + temp2[i] - 2.0*(td2 + ut2)*w[i] -
      4.0*thd1*wd1[i])/ang;

    crossp(wd2,u,wd2xu);

    for(i = 0;i < 3;i++)
      temp2[i] = thd2*u[i] + thd1*udot[i] - wd1[i];

    crossp(omega,temp2,temp1);

    crossp(alpha,temp0,temp2);

    for(i = 0;i < 3;i++)
      jerk[i] = thd3*u[i] + sa*wd2xu[i] - (1.0 - ca)*wd2[i] +
      2.0*thd2*udot[i] + thd1*((1.0 + ca)*wd1xu[i] - w2*u[i] - sa*wd1[i]) -
      wwd*sa*u[i] + temp1[i] + temp2[i];
  }
  else
  {
    crossp(th1,th2,temp1);

    for(i = 0;i < 3;i++)
    {
      omega[i] = th1[i];
      alpha[i] = th2[i];
       jerk[i] = th3[i] - 0.5*temp1[i];
    }
  }

}
Esempio n. 20
0
void CSolidView::OnMouse( MouseEvent& event )
{
	if(event.LeftDown() || event.MiddleDown() || event.RightDown())
	{
		m_button_down_point = Point(event.GetX(), event.GetY());
		m_current_point = m_button_down_point;
		//StoreViewPoint();
		m_initial_point = m_button_down_point;
	}
	else if(event.Dragging())
	{
		Point point_diff = Point(event.GetX(), event.GetY()) - m_current_point;

		if(event.LeftIsDown())
		{
			if(point_diff.x > 100)point_diff.x = 100;
			else if(point_diff.x < -100)point_diff.x = -100;
			if(point_diff.y > 100)point_diff.y = 100;
			else if(point_diff.y < -100)point_diff.y = -100;
			double c=(m_size.x+m_size.y)/20;

			double ang_x = point_diff.x/c;
			double ang_y = point_diff.y/c;
			
			double f[3];
			for(int i = 0; i<3; i++)f[i] = m_target_point[i] - m_lens_point[i];
			double fl = sqrt(f[0]*f[0] + f[1]*f[1] + f[2]*f[2]);
			double uu[3];
			for(int i = 0; i<3; i++)uu[i] = m_vertical[i];

			double r[3];
			crossp(f, uu, r);
			norm(r);

			double sinangx = sin(ang_x);
			double oneminuscosangx = 1 - cos(ang_x);
			double sinangy = sin(ang_y);
			double oneminuscosangy = 1 - cos(ang_y);

			for(int i = 0; i<3; i++)
			{
				m_lens_point[i] = m_lens_point[i] - r[i] * sinangx*fl;
				m_lens_point[i] = m_lens_point[i] + f[i] * oneminuscosangx;
				f[i] = f[i] + r[i] * sinangx * fl;
				f[i] = f[i] - f[i] * oneminuscosangx;
			}

			crossp(f, uu, r);
			norm(r);

			for(int i = 0; i<3; i++)
			{
				m_lens_point[i] = m_lens_point[i] + uu[i] * sinangy*fl;
				m_lens_point[i] = m_lens_point[i] + f[i] * oneminuscosangy;
				m_vertical[i] = m_vertical[i] + f[i] * sinangy/fl;
				m_vertical[i] = m_vertical[i] - uu[i] * oneminuscosangy;	
			}
			LimitCamera();
		}
		else if(event.MiddleIsDown())
		{
			double f[3];
			for(int i = 0; i<3; i++)f[i] = m_target_point[i] - m_lens_point[i];
			norm(f);
			double r[3];
			crossp(f, m_vertical, r);

			double d = dist(m_target_point, m_lens_point);

			double div_x = (double)(point_diff.x) * d * 0.001;
			double div_y = (double)(point_diff.y) * d * 0.001;
			for(int i = 0; i<3; i++)m_target_point[i] = m_target_point[i] - r[i] * div_x + m_vertical[i] * div_y;
			for(int i = 0; i<3; i++)m_lens_point[i] = m_lens_point[i] - r[i] * div_x + m_vertical[i] * div_y;

			LimitCamera();
		}

		Refresh();
		m_current_point = Point(event.GetX(), event.GetY());
	}

	if(event.GetWheelRotation() != 0)
	{
		double wheel_value = (double)(event.GetWheelRotation());
		double multiplier = -wheel_value /1000.0;
		ViewScale(multiplier);
		Refresh();
	}
}