Cylinder.h

Go to the documentation of this file.

#ifndef CYLINDER_HEADER
#define CYLINDER_HEADER
#include <stdio.h>
 
#include <istream>
#include <ostream>
#include <stdexcept>
#include <utility>
 
#include "LevMarFitting.h"
#include "LevMarLSWeight.h"
#include "PointCloud.h"
#include "basic.h"
#include <GfxTL/HyperplaneCoordinateSystem.h>
#include <MiscLib/NoShrinkVector.h>
#include <MiscLib/Vector.h>
 
#ifndef DLL_LINKAGE
#define DLL_LINKAGE
#endif
 
class DLL_LINKAGE Cylinder
{
public:
    struct ParallelNormalsError : public std::runtime_error
    {
        ParallelNormalsError();
    };
 
    enum
    {
        RequiredSamples = 2
    };
 
    Cylinder();
    Cylinder(const Vec3f& axisDir, const Vec3f& axisPos, float radius);
    Cylinder(const Vec3f& pointA, const Vec3f& pointB, const Vec3f& normalA, const Vec3f& normalB);
    bool Init(const MiscLib::Vector<Vec3f>& samples);
    bool InitAverage(const MiscLib::Vector<Vec3f>& samples);
    bool Init(const Vec3f& axisDir, const Vec3f& axisPos, float radius);
    bool Init(const Vec3f& pointA, const Vec3f& pointB, const Vec3f& normalA, const Vec3f& normalB);
    bool Init(bool binary, std::istream* i);
    void Init(FILE* i);
    void Init(float* array);
    inline float Distance(const Vec3f& p) const;
    inline void Normal(const Vec3f& p, Vec3f* normal) const;
    inline float DistanceAndNormal(const Vec3f& p, Vec3f* normal) const;
    inline float SignedDistance(const Vec3f& p) const;
    void Project(const Vec3f& p, Vec3f* pp) const;
    // parameters are (height, angle)
    void Parameters(const Vec3f& p, std::pair<float, float>* param) const;
    float Radius() const;
    float& Radius();
    const Vec3f& AxisDirection() const;
    Vec3f& AxisDirection();
    const Vec3f& AxisPosition() const;
    Vec3f& AxisPosition();
    const Vec3f AngularDirection() const;
    void RotateAngularDirection(float radians);
    bool LeastSquaresFit(const PointCloud& pc,
                         MiscLib::Vector<size_t>::const_iterator begin,
                         MiscLib::Vector<size_t>::const_iterator end);
    template <class IteratorT>
    bool LeastSquaresFit(IteratorT begin, IteratorT end);
 
    bool
    Fit(const PointCloud& pc,
        MiscLib::Vector<size_t>::const_iterator begin,
        MiscLib::Vector<size_t>::const_iterator end)
    {
        return LeastSquaresFit(pc, begin, end);
    }
 
    static bool Interpolate(const MiscLib::Vector<Cylinder>& cylinders,
                            const MiscLib::Vector<float>& weights,
                            Cylinder* ic);
    void Serialize(bool binary, std::ostream* o) const;
    static size_t SerializedSize();
    void Serialize(FILE* o) const;
    void Serialize(float* array) const;
    static size_t SerializedFloatSize();
 
    void Transform(float scale, const Vec3f& translate);
    void Transform(const GfxTL::MatrixXX<3, 3, float>& rot, const GfxTL::Vector3Df& trans);
    inline unsigned int
    Intersect(const Vec3f& p, const Vec3f& r, float* first, float* second) const;
 
private:
    template <class WeightT>
    class LevMarCylinder : public WeightT
    {
    public:
        enum
        {
            NumParams = 7
        };
 
        typedef float ScalarType;
 
        template <class IteratorT>
        ScalarType
        Chi(const ScalarType* params,
            IteratorT begin,
            IteratorT end,
            ScalarType* values,
            ScalarType* temp) const
        {
            ScalarType chi = 0;
            int size = end - begin;
#pragma omp parallel for schedule(static) reduction(+ : chi)
            for (int idx = 0; idx < size; ++idx)
            {
                Vec3f s;
                for (unsigned int j = 0; j < 3; ++j)
                {
                    s[j] = begin[idx][j] - params[j];
                }
                ScalarType u = params[5] * s[1] - params[4] * s[2];
                u *= u;
                ScalarType v = params[3] * s[2] - params[5] * s[0];
                u += v * v;
                v = params[4] * s[0] - params[3] * s[1];
                u += v * v;
                temp[idx] = std::sqrt(u);
                chi += (values[idx] = WeightT::Weigh(temp[idx] - params[6])) * values[idx];
            }
            return chi;
        }
 
        template <class IteratorT>
        void
        Derivatives(const ScalarType* params,
                    IteratorT begin,
                    IteratorT end,
                    const ScalarType* values,
                    const ScalarType* temp,
                    ScalarType* matrix) const
        {
            int size = end - begin;
#pragma omp parallel for schedule(static)
            for (int idx = 0; idx < size; ++idx)
            {
                Vec3f s;
                for (unsigned int j = 0; j < 3; ++j)
                {
                    s[j] = begin[idx][j] - params[j];
                }
                ScalarType g = s[0] * begin[idx][0] + s[1] * begin[idx][1] + s[2] * begin[idx][2];
                if (temp[idx] < 1e-6)
                {
                    matrix[idx * NumParams + 0] = std::sqrt(1 - params[3] * params[3]);
                    matrix[idx * NumParams + 1] = std::sqrt(1 - params[4] * params[4]);
                    matrix[idx * NumParams + 2] = std::sqrt(1 - params[5] * params[5]);
                }
                else
                {
                    matrix[idx * NumParams + 0] = (params[3] * g - s[0]) / temp[idx];
                    matrix[idx * NumParams + 1] = (params[4] * g - s[1]) / temp[idx];
                    matrix[idx * NumParams + 2] = (params[5] * g - s[2]) / temp[idx];
                }
                matrix[idx * NumParams + 3] = g * matrix[idx * NumParams + 0];
                matrix[idx * NumParams + 4] = g * matrix[idx * NumParams + 1];
                matrix[idx * NumParams + 5] = g * matrix[idx * NumParams + 2];
                matrix[idx * NumParams + 6] = -1;
                WeightT::template DerivWeigh<NumParams>(temp[idx] - params[6],
                                                        matrix + idx * NumParams);
            }
        }
 
        void
        Normalize(ScalarType* params) const
        {
            ScalarType l =
                std::sqrt(params[3] * params[3] + params[4] * params[4] + params[5] * params[5]);
            for (unsigned int i = 3; i < 6; ++i)
            {
                params[i] /= l;
            }
            // find point on axis closest to origin
            float lambda = -(params[0] * params[3] + params[1] * params[4] + params[2] * params[5]);
            for (unsigned int i = 0; i < 3; ++i)
            {
                params[i] = params[i] + lambda * params[i + 3];
            }
        }
    };
 
private:
    Vec3f m_axisDir;
    Vec3f m_axisPos;
    float m_radius;
    GfxTL::HyperplaneCoordinateSystem<float, 3> m_hcs;
    float m_angularRotatedRadians;
};
 
inline float
Cylinder::Distance(const Vec3f& p) const
{
    Vec3f diff = p - m_axisPos;
    float lambda = m_axisDir.dot(diff);
    float axisDist = (diff - lambda * m_axisDir).length();
    return abs(axisDist - m_radius);
}
 
inline void
Cylinder::Normal(const Vec3f& p, Vec3f* normal) const
{
    Vec3f diff = p - m_axisPos;
    float lambda = m_axisDir.dot(diff);
    *normal = diff - lambda * m_axisDir;
    normal->normalize();
}
 
inline float
Cylinder::DistanceAndNormal(const Vec3f& p, Vec3f* normal) const
{
    Vec3f diff = p - m_axisPos;
    float lambda = m_axisDir.dot(diff);
    *normal = diff - lambda * m_axisDir;
    float axisDist = normal->length();
    if (axisDist > 0)
    {
        *normal /= axisDist;
    }
    return abs(axisDist - m_radius);
}
 
inline float
Cylinder::SignedDistance(const Vec3f& p) const
{
    Vec3f diff = p - m_axisPos;
    float lambda = m_axisDir.dot(diff);
    float axisDist = (diff - lambda * m_axisDir).length();
    return axisDist - m_radius;
}
 
template <class IteratorT>
bool
Cylinder::LeastSquaresFit(IteratorT begin, IteratorT end)
{
    float param[7];
    for (size_t i = 0; i < 3; ++i)
    {
        param[i] = m_axisPos[i];
    }
    for (size_t i = 0; i < 3; ++i)
    {
        param[i + 3] = m_axisDir[i];
    }
    param[6] = m_radius;
    LevMarCylinder<LevMarLSWeight> levMarCylinder;
    if (!LevMar(begin, end, levMarCylinder, param))
    {
        return false;
    }
    for (size_t i = 0; i < 3; ++i)
    {
        m_axisPos[i] = param[i];
    }
    for (size_t i = 0; i < 3; ++i)
    {
        m_axisDir[i] = param[i + 3];
    }
    m_radius = param[6];
    m_hcs.FromNormal(m_axisDir);
    m_angularRotatedRadians = 0;
    return true;
}
 
inline unsigned int
Cylinder::Intersect(const Vec3f& p, const Vec3f& r, float* first, float* second) const
{
    using namespace std;
    // Create a coordinate system for the cylinder.  In this system, the
    // cylinder segment center C is the origin and the cylinder axis direction
    // W is the z-axis.  U and V are the other coordinate axis directions.
    // If P = x*U+y*V+z*W, the cylinder is x^2 + y^2 = r^2, where r is the
    // cylinder radius.  The end caps are |z| = h/2, where h is the cylinder
    // height.
    float fRSqr = m_radius * m_radius;
 
    // convert incoming line origin to cylinder coordinates
    Vec3f kDiff = p - m_axisPos;
    Vec3f kP(kDiff.dot(m_hcs[0]), kDiff.dot(m_hcs[1]), m_axisDir.dot(kDiff));
 
    // Get the z-value, in cylinder coordinates, of the incoming line's
    // unit-length direction.
    float fDz = m_axisDir.dot(r);
 
    if (abs(fDz) >= 1.f - 1e-7f)
    // The line is parallel to the cylinder axis.
    {
        return 0;
    }
 
    // convert incoming line unit-length direction to cylinder coordinates
    Vec3f kD(r.dot(m_hcs[0]), r.dot(m_hcs[1]), r.dot(m_axisDir));
 
    float fA0, fA1, fA2, fDiscr, fRoot, fInv;
 
    // Test intersection of line P+t*D with infinite cylinder
    // x^2+y^2 = r^2.  This reduces to computing the roots of a
    // quadratic equation.  If P = (px,py,pz) and D = (dx,dy,dz),
    // then the quadratic equation is
    //   (dx^2+dy^2)*t^2 + 2*(px*dx+py*dy)*t + (px^2+py^2-r^2) = 0
    fA0 = kP[0] * kP[0] + kP[1] * kP[1] - fRSqr;
    fA1 = kP[0] * kD[0] + kP[1] * kD[1];
    fA2 = kD[0] * kD[0] + kD[1] * kD[1];
    fDiscr = fA1 * fA1 - fA0 * fA2;
    if (fDiscr < 0)
    // line does not intersect cylinder
    {
        return 0;
    }
    else if (fDiscr > 1e-7f)
    {
        // line intersects cylinder in two places
        fRoot = sqrt(fDiscr);
        fInv = (1.f) / fA2;
        *first = (-fA1 - fRoot) * fInv;
        *second = (-fA1 + fRoot) * fInv;
        return 2;
    }
    // line is tangent to the cylinder
    *first = -fA1 / fA2;
    return 1;
}
 
#endif