.

utils/gfcsRotator.cpp

+#include "gfcsRotator.h"
+#include "BNOAstroBase.h"
+#include "GreenwichFrame.h"
+#include "LocalJ2000Frame.h"
+#include "NZEFrame.h"
+
+GFCSRotator::GFCSRotator(TimeJD baseDate) 
+{
+	Init(baseDate);
+}
+
+void GFCSRotator::Init(TimeJD baseDate)
+{
+	m_baseDate = baseDate;
+	IAstroBase *ab = GetAstroBase();
+	ab->MtxJ20002GFCS(baseDate, BJ2000toGFCS);
+}
+
+void GFCSRotator::Rotate(TimeJD date, TransMtx& CJ2000toGFCS, TransMtx& CGFCStoJ2000) const
+{
+	IAstroBase *ab = GetAstroBase();
+	double w = ab->GetEarthRotation();
+	double dt = (w * (date - m_baseDate)).GetDouble() * 86.4;
+	double cw = cos(dt);
+	double sw = sin(dt);
+	CJ2000toGFCS[0] = cw;
+	CJ2000toGFCS[1] = sw;
+	CJ2000toGFCS[2] = 0.0;
+	CJ2000toGFCS[3] = -sw;
+	CJ2000toGFCS[4] = cw;
+	CJ2000toGFCS[5] = 0.0;
+	CJ2000toGFCS[6] = 0.0;
+	CJ2000toGFCS[7] = 0.0;
+	CJ2000toGFCS[8] = 1.0;
+	CJ2000toGFCS *= BJ2000toGFCS;
+	CJ2000toGFCS.Transp(CGFCStoJ2000);
+}
+
+void GFCSRotator::GCSToJ2000(TimeJD date, const Vect6& gcs, Vect6& j2000) const
+{
+	TransMtx CJ2000toGFCS, CGFCStoJ2000;
+	Rotate(date, CJ2000toGFCS, CGFCStoJ2000);
+
+	Vect3 RGr;
+	RGr = gcs;
+	RGr *= CGFCStoJ2000;
+
+	double w = GetAstroBase()->GetEarthRotation();
+	Vect3 VGr;
+	gcs.GetVelocity(VGr);
+	VGr *= CGFCStoJ2000;
+	VGr.x -= w * RGr.y; 
+	VGr.y += w * RGr.x; 
+
+	j2000 = Vect6(RGr,VGr);
+}
+
+void GFCSRotator::J2000ToGCS(TimeJD date, const Vect6& j2000, Vect6& gcs) const
+{
+	//new short style but slower
+//	FrameConverter6D j2000ToGcs;
+//	GetGcsToJ20006dConverter(date, j2000ToGcs);
+//	j2000ToGcs.Invert();
+//	j2000ToGcs.Convert(j2000,gcs);
+
+	TransMtx CJ2000toGFCS, CGFCStoJ2000;
+	Rotate(date, CJ2000toGFCS, CGFCStoJ2000);
+
+	Vect3 RGr;
+	RGr = j2000;
+	RGr *= CJ2000toGFCS;
+
+	double w = GetAstroBase()->GetEarthRotation();
+	Vect3 VGr;
+	j2000.GetVelocity(VGr);
+	VGr *= CJ2000toGFCS;
+	VGr.x += w * RGr.y; 
+	VGr.y -= w * RGr.x; 
+
+	gcs = Vect6(RGr,VGr);
+}
+
+void GFCSRotator::GCSToJ2000(const PhasePoint6D& gcs, PhasePoint6D& j2000) const
+{
+	Vect6 pJ2000;
+	GCSToJ2000(gcs.T, gcs.CoordsVel, pJ2000);
+
+	J2000Frame fr;
+	j2000.Init(fr, pJ2000, gcs.T);
+}
+
+void GFCSRotator::J2000ToGCS(const PhasePoint6D& j2000, PhasePoint6D& gcs) const
+{
+	Vect6 pGcs;
+	J2000ToGCS(j2000.T, j2000.CoordsVel, pGcs);
+
+	GreenwichFrame fr;
+	gcs.Init(fr, pGcs, j2000.T);
+}
+
+void GFCSRotator::GetJ2000ToGcs6dConverter(TimeJD date, FrameConverter6D& j2000ToGcs) const
+{
+	TransMtx CJ2000toGFCS, CGFCStoJ2000;
+	Rotate(date, CJ2000toGFCS, CGFCStoJ2000);
+
+	j2000ToGcs.Init();
+	j2000ToGcs.Converter3D.Init(CJ2000toGFCS,Vect3());
+
+	TransMtx w;
+	GetAstroBase()->GCSVelocityMatrix(w);
+//	w.Transp();
+	
+	j2000ToGcs.Converter3D.Rotation.mul(w, j2000ToGcs.VRotation);
+}
+
+void GFCSRotator::GetGcsToJ20006dConverter(TimeJD date, FrameConverter6D& gcsToJ2000) const
+{
+	TransMtx CJ2000toGFCS, CGFCStoJ2000;
+	Rotate(date,CJ2000toGFCS, CGFCStoJ2000);
+
+	gcsToJ2000.Init();
+	gcsToJ2000.Converter3D.Init(CGFCStoJ2000,Vect3());
+
+	TransMtx w;
+	GetAstroBase()->GCSVelocityMatrix(w);
+	w.Transp();
+
+	gcsToJ2000.Converter3D.Rotation.mul(w, gcsToJ2000.VRotation);
+}
+
+void GFCSRotator::GetJ2000ToNze6dConverter(TimeJD date, Vect3 siteGcsPos, FrameConverter6D& J2000toNze) const
+{
+	NZEFrame nzeFrame;
+	nzeFrame.Init(siteGcsPos);
+
+	FrameConverter6D nzeToGcs;
+	nzeFrame.Get6DGreenwichConverter(nzeToGcs);
+
+	FrameConverter6D gcsToJ2000;
+	GetGcsToJ20006dConverter(date, gcsToJ2000);
+
+	//nzeToGcs * gcsToJ2000 = nzeToJ2000 -> J2000toNze
+	nzeToGcs.Multiply(gcsToJ2000, J2000toNze); 
+	J2000toNze.Invert();
+}
+
+void GFCSRotator::GetJ2000ToLocalJ20006dConverter(TimeJD date, Vect3 siteGcsPos, FrameConverter6D& J2000toLocalJ2000) const
+{
+	TransMtx CJ2000toGFCS, CGFCStoJ2000;
+	Rotate(date,CJ2000toGFCS, CGFCStoJ2000);
+	
+	//localJ2000ToJ2000 -> J2000toLocalJ2000
+	J2000toLocalJ2000.Init();
+	J2000toLocalJ2000.Converter3D.Shift = siteGcsPos;
+	J2000toLocalJ2000.Converter3D.Shift.MulLeft(CGFCStoJ2000);
+	
+	TransMtx w;
+	GetAstroBase()->GCSVelocityMatrix(w);
+	w.Transp();
+	
+	J2000toLocalJ2000.VShift = siteGcsPos;
+	J2000toLocalJ2000.VShift.MulLeft(w);
+	J2000toLocalJ2000.VShift.MulLeft(CGFCStoJ2000);
+	J2000toLocalJ2000.Invert();
+}
+
+void GFCSRotator::J2000ToNze(TimeJD date, const Vect6& j2000, Vect3 siteGcsPos, Vect6& nze) const
+{
+	FrameConverter6D J2000toNze;
+	GetJ2000ToNze6dConverter(date, siteGcsPos, J2000toNze);
+	J2000toNze.Convert(j2000,nze);
+}
+
+void GFCSRotator::J2000ToLocalJ2000(TimeJD date, const Vect6& j2000, Vect3 siteGcsPos, Vect6& localJ2000) const
+{
+	FrameConverter6D J2000toLocalJ2000;
+	GetJ2000ToLocalJ20006dConverter(date, siteGcsPos, J2000toLocalJ2000);
+	J2000toLocalJ2000.Convert(j2000, localJ2000);
+}
+

utils/gfcsRotator.h

+#pragma once
+
+#include "TM.h"
+#include "DVectors.h"
+#include "PhasePoint.h"
+
+class GFCSRotator
+{
+public:
+	GFCSRotator(){};
+	GFCSRotator(TimeJD baseDate);
+	void Init(TimeJD baseDate);
+	void Rotate(TimeJD date, TransMtx& CJ2000toGFCS, TransMtx& CGFCStoJ2000) const;
+	void J2000ToGCS(const PhasePoint6D& j2000, PhasePoint6D& gcs) const;
+	void J2000ToGCS(TimeJD date, const Vect6& j2000, Vect6& gcs) const;
+	void J2000ToLocalJ2000(TimeJD date, const Vect6& j2000, Vect3 siteGcsPos, Vect6& localJ2000) const;
+	void J2000ToNze(TimeJD date, const Vect6& j2000, Vect3 siteGcsPos, Vect6& nze) const;
+	void GCSToJ2000(const PhasePoint6D& gcs, PhasePoint6D& j2000) const;
+	void GCSToJ2000(TimeJD date, const Vect6& gcs, Vect6& j2000) const;	
+private:
+	void GetGcsToJ20006dConverter(TimeJD date, FrameConverter6D& gcsToJ2000) const;
+	void GetJ2000ToGcs6dConverter(TimeJD date, FrameConverter6D& j2000ToGcs) const;
+	void GetJ2000ToNze6dConverter(TimeJD date, Vect3 siteGcsPos, FrameConverter6D& gcsToJ2000) const;
+	void GetJ2000ToLocalJ20006dConverter(TimeJD date, Vect3 siteGcsPos, FrameConverter6D& J2000toLocalJ2000) const;
+	TransMtx BJ2000toGFCS;
+	TimeJD m_baseDate;
+};