GteApprGreatCircle3.h

Go to the documentation of this file.

// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2017
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)

#pragma once

#include <Mathematics/GteMatrix3x3.h>
#include <Mathematics/GteConstants.h>
#include <Mathematics/GteSymmetricEigensolver3x3.h>

namespace gte
{

// Least-squares fit of a great circle to unit-length vectors (x,y,z) by
// using distance measurements orthogonal (and measured along great circles)
// to the proposed great circle.  The inputs akPoint[] are unit length.  The
// returned value is unit length, call it N.  The fitted great circle is
// defined by Dot(N,X) = 0, where X is a unit-length vector on the great
// circle.
template <typename Real>
class ApprGreatCircle3
{
public:
    void operator()(int numPoints, Vector3<Real> const* points,
        Vector3<Real>& normal);
};


// In addition to the least-squares fit of a great circle, the input vectors
// are projected onto that circle.  The sector of smallest angle (possibly
// obtuse) that contains the points is computed.  The endpoints of the arc
// of the sector are returned.  The returned endpoints A0 and A1 are
// perpendicular to the returned normal N.  Moreover, when you view the
// arc by looking at the plane of the great circle with a view direction
// of -N, the arc is traversed counterclockwise starting at A0 and ending
// at A1.
template <typename Real>
class ApprGreatArc3
{
public:
    void operator()(int numPoints, Vector3<Real> const* points,
        Vector3<Real>& normal, Vector3<Real>& arcEnd0,
        Vector3<Real>& arcEnd1);
};


template <typename Real>
void ApprGreatCircle3<Real>::operator()(int numPoints,
    Vector3<Real> const* points, Vector3<Real>& normal)
{
    // Compute the covariance matrix of the vectors.
    Real covar00 = (Real)0, covar01 = (Real)0, covar02 = (Real)0;
    Real covar11 = (Real)0, covar12 = (Real)0, covar22 = (Real)0;
    for (int i = 0; i < numPoints; i++)
    {
        Vector3<Real> diff = points[i];
        covar00 += diff[0] * diff[0];
        covar01 += diff[0] * diff[1];
        covar02 += diff[0] * diff[2];
        covar11 += diff[1] * diff[1];
        covar12 += diff[1] * diff[2];
        covar22 += diff[2] * diff[2];
    }

    Real invNumPoints = ((Real)1) / static_cast<Real>(numPoints);
    covar00 *= invNumPoints;
    covar01 *= invNumPoints;
    covar02 *= invNumPoints;
    covar11 *= invNumPoints;
    covar12 *= invNumPoints;
    covar22 *= invNumPoints;

    // Solve the eigensystem.
    SymmetricEigensolver3x3<Real> es;
    std::array<Real, 3> eval;
    std::array<std::array<Real, 3>, 3> evec;
    es(covar00, covar01, covar02, covar11, covar12, covar22, false, +1,
        eval, evec);
    normal = evec[0];
}

template <typename Real>
void ApprGreatArc3<Real>::operator()(int numPoints,
    Vector3<Real> const* points, Vector3<Real>& normal,
    Vector3<Real>& arcEnd0, Vector3<Real>& arcEnd1)
{
    // Get the least-squares great circle for the vectors.  The circle is on
    // the plane Dot(N,X) = 0.  Generate a basis from N.
    Vector3<Real> basis[3];  // { N, U, V }
    ApprGreatCircle3<Real>()(numPoints, points, basis[0]);
    ComputeOrthogonalComplement(1, basis);

    // The vectors are X[i] = u[i]*U + v[i]*V + w[i]*N.  The projections
    // are P[i] = (u[i]*U + v[i]*V)/sqrt(u[i]*u[i] + v[i]*v[i]).  The great
    // circle is parameterized by C(t) = cos(t)*U + sin(t)*V.  Compute the
    // angles t in [-pi,pi] for the projections onto the great circle.  It
    // is not necesarily to normalize (u[i],v[i]), instead computing
    // t = atan2(v[i],u[i]).
    std::vector<std::array<Real, 3>> items(numPoints);  // item (u, v, angle)
    for (int i = 0; i < numPoints; ++i)
    {
        items[i][0] = Dot(basis[1], points[i]);
        items[i][1] = Dot(basis[2], points[i]);
        items[i][2] = atan2(items[i][1], items[i][0]);
    }
    std::sort(items.begin(), items.end(),
        [](std::array<Real, 3> const& item0, std::array<Real, 3> const& item1)
    {
        return item0[2] < item1[2];
    }
    );

    // Locate the pair of consecutive angles whose difference is a maximum.
    // Effectively, we are constructing a cone of minimum angle that contains
    // the unit-length vectors.
    int numPointsM1 = numPoints - 1;
    Real maxDiff = (Real)GTE_C_TWO_PI + items[0][2] - items[numPointsM1][2];
    int end0 = 0, end1 = numPointsM1;
    for (int i0 = 0, i1 = 1; i0 < numPointsM1; i0 = i1++)
    {
        Real diff = items[i1][2] - items[i0][2];
        if (diff > maxDiff)
        {
            maxDiff = diff;
            end0 = i1;
            end1 = i0;
        }
    }

    normal = basis[0];
    arcEnd0 = items[end0][0] * basis[1] + items[end0][1] * basis[2];
    arcEnd1 = items[end1][0] * basis[1] + items[end1][1] * basis[2];
    Normalize(arcEnd0);
    Normalize(arcEnd1);
}


}