AlignmentDatabase.cpp

#include "indidevapi.h" // For IDLog
#include "AlignmentDatabase.h"
#include <memory>
#include <cmath>
#include <sys/time.h>
#include <limits>
#include "Wm5DistSegment3Segment3.h"
#include "Wm5IntrSegment3Triangle3.h"
#include <fstream>


CAlignmentDatabase::CAlignmentDatabase()
{
    Wm5::Memory::Initialize();
    LowerCoordinateSystemUnits = HOURS_DEGREES;

#ifdef LOAD_TEST_DATA
    sleep(30); // Give time to attach debugger
    AddAlignmentPoint(5.619669, 0.03447, 0.506809, 1.732239, 1.463808);
    AddAlignmentPoint(5.659376, 0.618501, 1.557218, 5.427625, 0.611563);
    RecalculateDatabase();
#endif
}

CAlignmentDatabase::~CAlignmentDatabase()
{
    Wm5::Memory::Terminate("WM5MemoryDebug.txt");
}

bool CAlignmentDatabase::AddAlignmentPoint(const double ReferenceTime, const double RightAscension, const double Declination,
                                           const double Axis1, const double Axis2, bool Recalculate)
{
    // Save the new alignment point
    AlignmentPoints.push_back(AlignmentPoint(ReferenceTime, RightAscension, Declination, Axis1, Axis2));

    if (Recalculate)
        return RecalculateDatabase();
    else
        return true;
}

bool CAlignmentDatabase::RecalculateDatabase()
{
    Wm5::Vector3d* UpperDirectionCosines;
    Wm5::Vector3d* LowerDirectionCosines;
    std::auto_ptr<Wm5::ConvexHull3d> UpperLayerConvexHull;

    // TODO make this work with one and two alignment points
    if (AlignmentPoints.size() < 3)
    {
        IDLog("CAlignmentDatabase::RecalculateDatabase - Too few alignment points to generate matrices\n");
        return true;
    }

    // Allocate Vector3 arrays large enough to hold all the direction cosines
    UpperDirectionCosines = new Wm5::Vector3d[AlignmentPoints.size() + 1];
    LowerDirectionCosines = new Wm5::Vector3d[AlignmentPoints.size() + 1];

    // Set centre points to complete convex hulls
    UpperDirectionCosines[0] = Wm5::Vector3d(0.0, 0.0, 0.0);
    LowerDirectionCosines[0] = Wm5::Vector3d(0.0, 0.0, 0.0);

    // Iterate through all the alignment points generating new sets of rectangular coordinates (direction cosines) to give to the convex hull classes
    int iCount = 1;
    for (std::vector<AlignmentPoint>::iterator i = AlignmentPoints.begin() ; i != AlignmentPoints.end(); i++)
    {
        UpperDirectionCosines[iCount] = MakeUpperDirectionCosine((*i).RightAscension, (*i).Declination, (*i).ReferenceTime);

        LowerDirectionCosines[iCount] = MakeLowerDirectionCosine((*i).Axis1, (*i).Axis2, (*i).ReferenceTime);

        iCount++;
    }


    // Set up the hulls
    UpperLayerConvexHull.reset(new Wm5::ConvexHull3d(iCount, UpperDirectionCosines, Epsilon, false, Wm5::Query::QT_RATIONAL));

    // Sanity checks
    int UpperLayerDimension = UpperLayerConvexHull->GetDimension();
    int UpperLayerFacets = UpperLayerConvexHull->GetNumSimplices();
    const int *UpperIndices = UpperLayerConvexHull->GetIndices();
    if (UpperLayerDimension < 3)
    {
        IDLog("CAlignmentDatabase::AddAlignmentPoint - UpperLayerConvexHull not 3d (%d)\n", UpperLayerDimension);
        delete[] UpperDirectionCosines;
        delete[] LowerDirectionCosines;
        return false;
    }

    // Reset the database
    UpperLayerTinFacets.clear();
    LowerLayerTinFacets.clear();

    // Iterate through the facets of the upper layer convex hull;
    for (int i = 0; i < UpperLayerFacets; i++)
    {
        TinFacet UpperLayerFacet;
        TinFacet LowerLayerFacet;

        // Construct the triangular facets
        UpperLayerFacet.Facet = Wm5::Triangle3d(UpperDirectionCosines[UpperIndices[3 * i + 0]],
                                                UpperDirectionCosines[UpperIndices[3 * i + 1]],
                                                UpperDirectionCosines[UpperIndices[3 * i + 2]]);
        LowerLayerFacet.Facet = Wm5::Triangle3d(LowerDirectionCosines[UpperIndices[3 * i + 0]],
                                                LowerDirectionCosines[UpperIndices[3 * i + 1]],
                                                LowerDirectionCosines[UpperIndices[3 * i + 2]]);

        // Compute the matrices

        Wm5::Vector3d UpperVertex1 = UpperLayerFacet.Facet.V[0];
        Wm5::Vector3d UpperVertex2 = UpperLayerFacet.Facet.V[1];
        Wm5::Vector3d UpperVertex3 = UpperLayerFacet.Facet.V[2];
        Wm5::Vector3d LowerVertex1 = LowerLayerFacet.Facet.V[0];
        Wm5::Vector3d LowerVertex2 = LowerLayerFacet.Facet.V[1];
        Wm5::Vector3d LowerVertex3 = LowerLayerFacet.Facet.V[2];

        if ((UpperVertex1 == Wm5::Vector3d::ZERO) ||
            (UpperVertex2 == Wm5::Vector3d::ZERO) ||
            (UpperVertex3 == Wm5::Vector3d::ZERO))
        {
            // One of the vertices is the origin. So this facet does not lie on the
            // surface of the unit sphere. However the two vertices that are not the
            // origin will define a line segment of the border of the triangulated area (TIN)
            // Compute an artificially generated third point (vector cross product) to replace
            // the origin vertex
            Wm5::Segment3d BoundarySegment;
            if (UpperVertex1 == Wm5::Vector3d::ZERO)
            {
                UpperVertex1 = UpperVertex2.UnitCross(UpperVertex3);
                LowerVertex1 = LowerVertex2.UnitCross(LowerVertex3);
            }
            else if (UpperVertex2 == Wm5::Vector3d::ZERO)
            {
                UpperVertex2 = UpperVertex1.UnitCross(UpperVertex3);
                LowerVertex2 = LowerVertex1.UnitCross(LowerVertex3);
            }
            else
            {
                UpperVertex3 = UpperVertex1.UnitCross(UpperVertex2);
                LowerVertex3 = LowerVertex1.UnitCross(LowerVertex2);
            }
        }

        Wm5::Matrix3d UpperMatrix(UpperVertex1.X(), UpperVertex2.X(), UpperVertex3.X(),
                                  UpperVertex1.Y(), UpperVertex2.Y(), UpperVertex3.Y(),
                                  UpperVertex1.Z(), UpperVertex2.Z(), UpperVertex3.Z());
        Wm5::Matrix3d LowerMatrix(LowerVertex1.X(), LowerVertex2.X(), LowerVertex3.X(),
                                  LowerVertex1.Y(), LowerVertex2.Y(), LowerVertex3.Y(),
                                  LowerVertex1.Z(), LowerVertex2.Z(), LowerVertex3.Z());
        UpperLayerFacet.TransformationMatrix = LowerMatrix * UpperMatrix.Inverse();
        LowerLayerFacet.TransformationMatrix = UpperLayerFacet.TransformationMatrix.Inverse();

        // Update the database
        UpperLayerTinFacets.push_back(UpperLayerFacet);
        LowerLayerTinFacets.push_back(LowerLayerFacet);
    }
    delete[] UpperDirectionCosines;
    delete[] LowerDirectionCosines;
    return true;
}

bool CAlignmentDatabase::TransformUpperToLower(const double RightAscension, const double Declination, double& Axis1, double& Axis2)
{
    const double ApproximateGMST = UTCNow();

    Wm5::Vector3d UpperDirectionCosine = MakeUpperDirectionCosine(RightAscension, Declination, ApproximateGMST);

    Wm5::Matrix3d TransformationMatrix = FindTransformationmatrix(UpperDirectionCosine, UPPER_TO_LOWER);

    Wm5::Vector3d LowerDirectionCosine = TransformationMatrix * UpperDirectionCosine;

    double Polar1 = atan2(LowerDirectionCosine[1], LowerDirectionCosine[0]); // Radians minus PI to plus PI
    double Polar2 = asin(LowerDirectionCosine[2]); // Radians minus PI/2 to plus PI/2

    switch (LowerCoordinateSystemUnits)
    {
        case HOURS_DEGREES:
        {
            Axis1 = DummyHourAngleToRightAscension(Polar1, ApproximateGMST);
            Axis2 = Polar2 * 180.0 / M_PI;
            break;
        }
        case DEGREES_DEGREES:
        {
            Axis1 = Polar1 * 180.0 / M_PI;
            Axis2 = Polar2 * 180.0 / M_PI;
            break;
        }
        default:
        case RADIANS_RADIANS:
        {
            Axis1 = Polar1;
            Axis2 = Polar2;
            break;
        }
    }

    IDLog("CAlignmentDatabase::TransformUpperToLower RightAscension %g Declination %g Axis1 %g Axis2 %g\n", RightAscension,
                                                                                Declination,
                                                                                Axis1,
                                                                                Axis2);

    return true;
}

bool CAlignmentDatabase::TransformLowerToUpper(const double Axis1, const double Axis2, double& RightAscension, double& Declination)
{
    const double ApproximateGMST = UTCNow();
    Wm5::Vector3d LowerDirectionCosine = MakeLowerDirectionCosine(Axis1, Axis2, ApproximateGMST);

    Wm5::Matrix3d TransformationMatrix = FindTransformationmatrix(LowerDirectionCosine, LOWER_TO_UPPER);

    Wm5::Vector3d UpperDirectionCosine = TransformationMatrix * LowerDirectionCosine;
    RightAscension = DummyHourAngleToRightAscension(atan2(UpperDirectionCosine[1], UpperDirectionCosine[0]), ApproximateGMST);
    Declination = asin(UpperDirectionCosine[2]) * 180.0 / M_PI; // minus PI/2 to plus PI/2

    IDLog("CAlignmentDatabase::TransformLowerToUpper Axis1 %g Axis2 %g RightAscension %g Declination %g\n", Axis1,
                                                                                Axis2,
                                                                                RightAscension,
                                                                                Declination);

    return true;
}

double CAlignmentDatabase::RightAscensionToDummyHourAngle(const double RightAscension, const double ReferenceTime)
{
    double DummyHourAngle =  (RightAscension * M_PI / 12.0) - 1.002737908 * (ReferenceTime * M_PI / 12.0);
    IDLog("CAlignmentDatabase::RightAscensionToDummyHourAngle - RightAscension %g ReferenceTime %g DummyHourAngle %g\n", RightAscension,
                                                                                                                        ReferenceTime,
                                                                                                                        DummyHourAngle);
    return DummyHourAngle;
}

double CAlignmentDatabase::DummyHourAngleToRightAscension(const double DummyHourAngle, const double ReferenceTime)
{
    // Normalise the hour angle;
    double NormalisedHourAngle;
    if (DummyHourAngle < 0)
        NormalisedHourAngle = 2 * M_PI + DummyHourAngle;
    else
        NormalisedHourAngle = DummyHourAngle;
    double RightAscension = (NormalisedHourAngle * 12.0 / M_PI) + 1.002737908 * ReferenceTime;
    IDLog("CAlignmentDatabase::DummyHourAngleToRightAscension - DummyHourAngle %g ReferenceTime %g RightAscension %g\n", DummyHourAngle,
                                                                                                                        ReferenceTime,
                                                                                                                        RightAscension);
    return RightAscension;
}

Wm5::Vector3d CAlignmentDatabase::MakeUpperDirectionCosine(const double RightAscension, const double Declination, const double ReferenceTime)
{
    double DeclinationInRadians = Declination * M_PI / 180.0;
    return Wm5::Vector3d(cos(DeclinationInRadians) * cos(RightAscensionToDummyHourAngle(RightAscension, ReferenceTime)),
                        cos(DeclinationInRadians) * sin(RightAscensionToDummyHourAngle(RightAscension, ReferenceTime)),
                        sin(DeclinationInRadians));
}

Wm5::Vector3d CAlignmentDatabase::MakeLowerDirectionCosine(const double Axis1, const double Axis2, const double ReferenceTime)
{
    switch (LowerCoordinateSystemUnits)
    {
        case HOURS_DEGREES:
        {
            double RA = RightAscensionToDummyHourAngle(Axis1, ReferenceTime);
            double Dec = Axis2 * M_PI / 180.0;
            return Wm5::Vector3d(cos(Dec) * cos(RA), cos(Dec) * sin(RA), sin(Dec));
        }
        case DEGREES_DEGREES:
        {

            double Azimuth = -Axis1 * M_PI / 180.0;
            double Altitude = Axis2 * M_PI / 180.0;
            return Wm5::Vector3d(cos(Altitude) * cos(Azimuth), cos(Altitude) * sin(Azimuth), sin(Altitude));
        }
        default:
        case RADIANS_RADIANS:
        {
            double Azimuth = -Axis1;
            double Altitude = Axis2;
            return Wm5::Vector3d(cos(Altitude) * cos(Azimuth), cos(Altitude) * sin(Azimuth), sin(Altitude));
        }
    }
}

const double CAlignmentDatabase::UTCNow()
{
    // I would really like this to be UT1Now but that would be harder
    // this will be within 0.9 seconds of UT1
    struct timeval now;
    gettimeofday(&now,NULL);
    struct tm breakdown = *gmtime(&now.tv_sec);
    return breakdown.tm_hour + breakdown.tm_min / 60.0 + breakdown.tm_sec / 3600.0 + now.tv_usec / 3600000000.0;
}

const Wm5::Matrix3d CAlignmentDatabase::FindTransformationmatrix(const Wm5::Vector3d DirectionCosine, Direction Direction)
{
    Wm5::Matrix3d TransformationMatrix;

    TinFacets& Facets = Direction == UPPER_TO_LOWER ? UpperLayerTinFacets : LowerLayerTinFacets;

    // Make the direction cosine (should be a unit vector) a bit longer just in case.
    Wm5::Segment3d DirectionSegment(Wm5::Vector3d::ZERO, DirectionCosine * 2.0);

    // The facets should all approximate to a patch on the surface of a unit sphere centered on the origin.
    // The only exception being those facets with the origin as a vertex.
    // So iterate through the facets and for each facet that has the origin as vertex check the distance of closest approach
    // between my DirectionVector and the line segment formed by the other two facet vertices. If the facet does not have the
    // origin as vertex check whether the vector intersects the surface patch. If the vector intersects then return the
    // TrasnformationMatrix from that facet. If I reach the end without intersecting the return the TransformationMatrix from the
    // origin facet the vector passes nearest to (whew!).

    TinFacets::iterator NearestOriginFacet;
    double DistanceToNearestOriginSegment = std::numeric_limits<double>::max();

    for (TinFacets::iterator i = Facets.begin(); i != Facets.end(); i++)
    {
        Wm5::Vector3d Vertex1 = (*i).Facet.V[0];
        Wm5::Vector3d Vertex2 = (*i).Facet.V[1];
        Wm5::Vector3d Vertex3 = (*i).Facet.V[2];

        if ((Vertex1 == Wm5::Vector3d::ZERO) ||
            (Vertex2 == Wm5::Vector3d::ZERO) ||
            (Vertex3 == Wm5::Vector3d::ZERO))
        {
            Wm5::Segment3d BoundarySegment;
            if (Vertex1 == Wm5::Vector3d::ZERO)
                BoundarySegment = Wm5::Segment3d(Vertex2, Vertex3);
            else if (Vertex2 == Wm5::Vector3d::ZERO)
                BoundarySegment = Wm5::Segment3d(Vertex1, Vertex3);
            else
                BoundarySegment = Wm5::Segment3d(Vertex1, Vertex2);
            Wm5::DistSegment3Segment3d Distance(DirectionSegment, BoundarySegment);
            double DistanceToSegment = Distance.Get();
            if (DistanceToSegment < DistanceToNearestOriginSegment)
            {
                DistanceToNearestOriginSegment = DistanceToSegment;
                NearestOriginFacet = i;
            }
        }
        else
        {
            Wm5::IntrSegment3Triangle3d Intersection(DirectionSegment, (*i).Facet);
            if (Intersection.Test())
                return (*i).TransformationMatrix;
        }
    }

    if (std::numeric_limits<double>::max() == DistanceToNearestOriginSegment)
    {
        IDLog("CAlignmentDatabase::FindTransformationmatrix - FATAL cannot find transformation matrix\n");
        return Wm5::Matrix3d::IDENTITY;
    }
    return (*NearestOriginFacet).TransformationMatrix;
}

bool CAlignmentDatabase::Save(const std::string& Filename)
{
    std::fstream SavedAlignmentPoints(Filename.c_str(), std::ios_base::out);

    for (std::vector<AlignmentPoint>::iterator i = AlignmentPoints.begin() ; i != AlignmentPoints.end() && !SavedAlignmentPoints.fail(); i++)
    {
        SavedAlignmentPoints << (*i).ReferenceTime << " "
                            << (*i).RightAscension << " "
                            << (*i).Declination << " "
                            << (*i).Axis1 << " "
                            << (*i).Axis2 << std::endl;
    }
    if (SavedAlignmentPoints.fail())
        return false;
    SavedAlignmentPoints.close();
    return true;
}

bool CAlignmentDatabase::Load(const std::string& Filename)
{
    std::vector<AlignmentPoint> NewAlignmentPoints;
    std::fstream SavedAlignmentPoints(Filename.c_str(), std::ios_base::in);
    if (!SavedAlignmentPoints)
    {
        IDLog("CAlignmentDatabase::Load - Failed to open saved points file (%s)\n", Filename.c_str());
        return false;
    }
    double ReferenceTime;
    while (SavedAlignmentPoints >> ReferenceTime)
    {
        double RightAscension, Declination, Axis1, Axis2;

        // Not sure if I should test for eof here - I never really understood iostreams

        // Read rest of line
        SavedAlignmentPoints >> RightAscension >> Declination >> Axis1 >> Axis2;

        if (!SavedAlignmentPoints.eof() && !SavedAlignmentPoints.fail())
            NewAlignmentPoints.push_back(AlignmentPoint(ReferenceTime, RightAscension, Declination, Axis1, Axis2));
    }
    if (!SavedAlignmentPoints.eof())
    {
        IDLog("CAlignmentDatabase::Load - Failed to read saved points from file (%s)\n", Filename.c_str());
        return false;
    }
    SavedAlignmentPoints.close();
    AlignmentPoints = NewAlignmentPoints;
    RecalculateDatabase();
    return true;
}

bool CAlignmentDatabase::Append(const std::string& Filename)
{
    std::vector<AlignmentPoint> NewAlignmentPoints;
    std::fstream SavedAlignmentPoints(Filename.c_str(), std::ios_base::in);
    if (!SavedAlignmentPoints)
    {
        IDLog("CAlignmentDatabase::Append - Failed to open saved points file (%s)\n", Filename.c_str());
        return false;
    }
    double ReferenceTime;
    while (SavedAlignmentPoints >> ReferenceTime)
    {
        double RightAscension, Declination, Axis1, Axis2;

        // Not sure if I should test for eof here - I never really understood iostreams

        // Read rest of line
        SavedAlignmentPoints >> RightAscension >> Declination >> Axis1 >> Axis2;

        if (!SavedAlignmentPoints.eof() && !SavedAlignmentPoints.fail())
            NewAlignmentPoints.push_back(AlignmentPoint(ReferenceTime, RightAscension, Declination, Axis1, Axis2));
    }
    if (!SavedAlignmentPoints.eof())
    {
        IDLog("CAlignmentDatabase::Append - Failed to read saved points from file (%s)\n", Filename.c_str());
        return false;
    }
    SavedAlignmentPoints.close();
    AlignmentPoints.insert(AlignmentPoints.end(), NewAlignmentPoints.begin(), NewAlignmentPoints.end());
    RecalculateDatabase();
    return true;
}