Sphere.cpp

Go to the documentation of this file.

#include "Sphere.h"
#include <MiscLib/Performance.h>
#include <GfxTL/IndexedIterator.h>
#include <GfxTL/MathHelper.h>
#ifdef DOPARALLEL
#include <omp.h>
#endif
 
extern int dmat_solve(int n, int rhs_num, double a[]);
 
void tetrahedron_circumsphere_3d(double tetra[3 * 4], double* r, double pc[3])
 
//******************************************************************************
//
//  Purpose:
//
//    TETRAHEDRON_CIRCUMSPHERE_3D computes the circumsphere of a tetrahedron in 3D.
//
//  Discussion:
//
//    The circumsphere, or circumscribed sphere, of a tetrahedron is the sphere that
//    passes through the four vertices.  The circumsphere is not necessarily
//    the smallest sphere that contains the tetrahedron.
//
//    Surprisingly, the diameter of the sphere can be found by solving
//    a 3 by 3 linear system.  This is because the vectors P2 - P1,
//    P3 - P1 and P4 - P1 are secants of the sphere, and each forms a
//    right triangle with the diameter through P1.  Hence, the dot product of
//    P2 - P1 with that diameter is equal to the square of the length
//    of P2 - P1, and similarly for P3 - P1 and P4 - P1.  This determines
//    the diameter vector originating at P1, and hence the radius and
//    center.
//
//  Modified:
//
//    10 August 2005
//
//  Author:
//
//    John Burkardt
//
//  Reference:
//
//    Adrian Bowyer and John Woodwark,
//    A Programmer's Geometry,
//    Butterworths, 1983.
//
//  Parameters:
//
//    Input, double TETRA[3*4], the vertices of the tetrahedron.
//
//    Output, double *R, PC[3], the coordinates of the center of the
//    circumscribed sphere, and its radius.  If the linear system is
//    singular, then R = -1, PC[] = 0.
//
{
# define DIM_NUM 3
# define RHS_NUM 1
 
    double a[DIM_NUM * (DIM_NUM + RHS_NUM)];
    int info;
    //
    //  Set up the linear system.
    //
    a[0 + 0 * 3] = tetra[0 + 1 * 3] - tetra[0 + 0 * 3];
    a[0 + 1 * 3] = tetra[1 + 1 * 3] - tetra[1 + 0 * 3];
    a[0 + 2 * 3] = tetra[2 + 1 * 3] - tetra[2 + 0 * 3];
    a[0 + 3 * 3] = std::pow(tetra[0 + 1 * 3] - tetra[0 + 0 * 3], 2)
                   + std::pow(tetra[1 + 1 * 3] - tetra[1 + 0 * 3], 2)
                   + std::pow(tetra[2 + 1 * 3] - tetra[2 + 0 * 3], 2);
 
    a[1 + 0 * 3] = tetra[0 + 2 * 3] - tetra[0 + 0 * 3];
    a[1 + 1 * 3] = tetra[1 + 2 * 3] - tetra[1 + 0 * 3];
    a[1 + 2 * 3] = tetra[2 + 2 * 3] - tetra[2 + 0 * 3];
    a[1 + 3 * 3] = std::pow(tetra[0 + 2 * 3] - tetra[0 + 0 * 3], 2)
                   + std::pow(tetra[1 + 2 * 3] - tetra[1 + 0 * 3], 2)
                   + std::pow(tetra[2 + 2 * 3] - tetra[2 + 0 * 3], 2);
 
    a[2 + 0 * 3] = tetra[0 + 3 * 3] - tetra[0 + 0 * 3];
    a[2 + 1 * 3] = tetra[1 + 3 * 3] - tetra[1 + 0 * 3];
    a[2 + 2 * 3] = tetra[2 + 3 * 3] - tetra[2 + 0 * 3];
    a[2 + 3 * 3] = std::pow(tetra[0 + 3 * 3] - tetra[0 + 0 * 3], 2)
                   + std::pow(tetra[1 + 3 * 3] - tetra[1 + 0 * 3], 2)
                   + std::pow(tetra[2 + 3 * 3] - tetra[2 + 0 * 3], 2);
    //
    //  Solve the linear system.
    //
    info = dmat_solve(DIM_NUM, RHS_NUM, a);
    //
    //  If the system was singular, return a consolation prize.
    //
    if (info != 0)
    {
        *r = -1.0;
        for (size_t i = 0; i < DIM_NUM; ++i)
        {
            pc[i] = 0;
        }
        //dvec_zero ( DIM_NUM, pc );
        return;
    }
    //
    //  Compute the radius and center.
    //
    *r = 0.5 * std::sqrt
         (a[0 + 3 * 3] * a[0 + 3 * 3]
          + a[1 + 3 * 3] * a[1 + 3 * 3]
          + a[2 + 3 * 3] * a[2 + 3 * 3]);
 
    pc[0] = tetra[0 + 0 * 3] + 0.5 * a[0 + 3 * 3];
    pc[1] = tetra[1 + 0 * 3] + 0.5 * a[1 + 3 * 3];
    pc[2] = tetra[2 + 0 * 3] + 0.5 * a[2 + 3 * 3];
 
    return;
# undef DIM_NUM
# undef RHS_NUM
}
 
bool Midpoint(const Vec3f& p1, const Vec3f& n1, const Vec3f& p2, const Vec3f& n2,
              Vec3f* mid)
{
    float d1343, d4321, d1321, d4343, d2121;
    float numer, denom, mua, mub;
 
    Vec3f p13 = p1 - p2;
    // p43 = n2
    // p21 = n1
    d1343 = p13[0] * n2[0] + p13[1] * n2[1] + p13[2] * n2[2];
    d4321 = n2[0] * n1[0] + n2[1] * n1[1] + n2[2] * n1[2];
    d1321 = p13[0] * n1[0] + p13[1] * n1[1] + p13[2] * n1[2];
    d4343 = n2[0] * n2[0] + n2[1] * n2[1] + n2[2] * n2[2];
    d2121 = n1[0] * n1[0] + n1[1] * n1[1] + n1[2] * n1[2];
 
    denom = d2121 * d4343 - d4321 * d4321;
    if (abs(denom) < 1e-6)
    {
        return false;
    }
    numer = d1343 * d4321 - d1321 * d4343;
 
    mua = numer / denom;
    mub = (d1343 + d4321 * (mua)) / d4343;
 
    Vec3f pa, pb;
    pa = p1 + mua * n1;
    pb = p2 + mub * n2;
    *mid = 0.5f * (pa + pb);
    return true;
}
 
InvalidTetrahedonError::InvalidTetrahedonError()
    : std::runtime_error("Invalid tetrahedon")
{}
 
Sphere::Sphere()
{}
 
Sphere::Sphere(const Vec3f& center, float radius)
    : m_center(center)
    , m_radius(radius)
{}
 
Sphere::Sphere(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
               const Vec3f& p4)
{
    if (!Init(p1, p2, p3, p4))
    {
        throw InvalidTetrahedonError();
    }
}
 
bool Sphere::Init(const MiscLib::Vector< Vec3f >& samples)
{
    if (samples.size() < 4)
    {
        return false;
    }
    // get center
    size_t c = samples.size() / 2;
    m_center = Vec3f(0, 0, 0);
    size_t midCount = 0;
    for (size_t i = 0; i < c - 1; ++i)
        for (size_t j = i + 1; j < c; ++j)
        {
            Vec3f mid;
            if (!Midpoint(samples[i], samples[i + c], samples[j], samples[j + c], &mid))
            {
                continue;
            }
            m_center += mid;
            ++midCount;
        }
    if (!midCount)
    {
        return false;
    }
    m_center /= midCount;
    m_radius = 0;
    for (size_t i = 0; i < c; ++i)
    {
        float d = (samples[i] - m_center).length();
        m_radius += d;
    }
    m_radius /= c;
    return true;
}
 
bool Sphere::Init(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
                  const Vec3f& p4)
{
    // convert to double array
    double tetra[4 * 3];
    for (size_t i = 0; i < 3; ++i)
    {
        tetra[0 * 3 + i] = p1[i];
    }
    for (size_t i = 0; i < 3; ++i)
    {
        tetra[1 * 3 + i] = p2[i];
    }
    for (size_t i = 0; i < 3; ++i)
    {
        tetra[2 * 3 + i] = p3[i];
    }
    for (size_t i = 0; i < 3; ++i)
    {
        tetra[3 * 3 + i] = p4[i];
    }
    double r, pc[3];
    tetrahedron_circumsphere_3d(tetra, &r, pc);
    if (r < 0)
    {
        return false;
    }
    m_radius = r;
    m_center[0] = pc[0];
    m_center[1] = pc[1];
    m_center[2] = pc[2];
    return true;
}
 
bool Sphere::Init2(const Vec3f& p1, const Vec3f& p2, const Vec3f& n1,
                   const Vec3f& n2)
{
    /*
       Calculate the line segment PaPb that is the shortest route between
       two lines P1P2 and P3P4. Calculate also the values of mua and mub where
          Pa = P1 + mua (P2 - P1)
          Pb = P3 + mub (P4 - P3)
       Return FALSE if no solution exists.
    */
    float d1343, d4321, d1321, d4343, d2121;
    float numer, denom, mua, mub;
 
    Vec3f p13 = p1 - p2;
    // p43 = n2
    // p21 = n1
    d1343 = p13[0] * n2[0] + p13[1] * n2[1] + p13[2] * n2[2];
    d4321 = n2[0] * n1[0] + n2[1] * n1[1] + n2[2] * n1[2];
    d1321 = p13[0] * n1[0] + p13[1] * n1[1] + p13[2] * n1[2];
    d4343 = n2[0] * n2[0] + n2[1] * n2[1] + n2[2] * n2[2];
    d2121 = n1[0] * n1[0] + n1[1] * n1[1] + n1[2] * n1[2];
 
    denom = d2121 * d4343 - d4321 * d4321;
    if (abs(denom) < 1e-6)
    {
        return false;
    }
    numer = d1343 * d4321 - d1321 * d4343;
 
    mua = numer / denom;
    mub = (d1343 + d4321 * (mua)) / d4343;
 
    Vec3f pa, pb;
    pa = p1 + mua * n1;
    pb = p2 + mub * n2;
 
    // get the midpoint between pa and pb and make it the center
    m_center = 0.5f * (pa + pb);
    // make the radius the average of the two distances
    float da = (p1 - m_center).length();
    float db = (p2 - m_center).length();
    m_radius = 0.5f * (da + db);
    // do some plausability checks
    // lets say the actual distance should not deviate by more than 10%
    float dev = da / m_radius;
    if (dev < 0.9f || dev > 1.1f)
    {
        return false;
    }
    dev = db / m_radius;
    if (dev < 0.9f || dev > 1.1f)
    {
        return false;
    }
    // distance between pa and pb should not be greater than 10% of the radius
    dev = (pa - pb).length() / m_radius;
    if (dev > 0.1f)
    {
        return false;
    }
    return true;
}
 
bool Sphere::Init(bool binary, std::istream* i)
{
    if (binary)
    {
        i->read((char*)&m_center, sizeof(m_center));
        i->read((char*)&m_radius, sizeof(m_radius));
    }
    else
    {
        for (size_t j = 0; j < 3; ++j)
        {
            (*i) >> m_center[j];
        }
        (*i) >> m_radius;
    }
    return true;
}
 
void Sphere::Init(FILE* i)
{
    fread(&m_center, sizeof(m_center), 1, i);
    fread(&m_radius, sizeof(m_radius), 1, i);
}
 
void Sphere::Init(float* array)
{
    for (int i = 0; i < 3; i++)
    {
        m_center[i] = array[i];
    }
    m_radius = array[3];
}
 
void Sphere::Project(const Vec3f& p, Vec3f* pp) const
{
    *pp = p - m_center;
    float l = pp->length();
    *pp *= m_radius / l;
    *pp += m_center;
}
 
const Vec3f& Sphere::Center() const
{
    return m_center;
}
 
float Sphere::Radius() const
{
    return m_radius;
}
 
float SphereDistance(const float* param, const float* x)
{
    float s = x[0] - param[0];
    s *= s;
    for (unsigned int i = 1; i < 3; ++i)
    {
        float ss = x[i] - param[i];
        s += ss * ss;
    }
    return std::sqrt(s) - param[3];
}
 
void SphereDistanceDerivatives(const float* param, const float* x,
                               float* gradient)
{
    float s[3];
    s[0] = x[0] - param[0];
    float sl = s[0] * s[0];
    for (unsigned int i = 1; i < 3; ++i)
    {
        s[i] = x[i] - param[i];
        sl += s[i] * s[i];
    }
    sl = std::sqrt(sl);
    gradient[0] = -s[0] / sl;
    gradient[1] = -s[1] / sl;
    gradient[2] = -s[2] / sl;
    gradient[3] = -1;
}
 
void NormalizeSphereParams(float* param)
{}
 
bool Sphere::LeastSquaresFit(const PointCloud& pc,
                             MiscLib::Vector< size_t >::const_iterator begin,
                             MiscLib::Vector< size_t >::const_iterator end)
{
    bool retVal = LeastSquaresFit(GfxTL::IndexIterate(begin, pc.begin()),
                                  GfxTL::IndexIterate(end, pc.begin()));
    return retVal;
}
 
bool Sphere::Interpolate(const MiscLib::Vector< Sphere >& spheres,
                         const MiscLib::Vector< float >& weights, Sphere* is)
{
    Vec3f center(0, 0, 0);
    float radius = 0;
    for (size_t i = 0; i < spheres.size(); ++i)
    {
        center += weights[i] * spheres[i].Center();
        radius += weights[i] * spheres[i].Radius();
    }
    is->Center(center);
    is->Radius(radius);
    return true;
}
 
void Sphere::Serialize(bool binary, std::ostream* o) const
{
    if (binary)
    {
        o->write((const char*)&m_center, sizeof(m_center));
        o->write((const char*)&m_radius, sizeof(m_radius));
    }
    else
    {
        (*o) << m_center[0] << " " << m_center[1] << " " << m_center[2] << " "
             << m_radius << " ";
    }
}
 
size_t Sphere::SerializedSize()
{
    return sizeof(Vec3f)
           + sizeof(float);
}
 
size_t Sphere::SerializedFloatSize()
{
    return 4;
}
 
void Sphere::Serialize(FILE* o) const
{
    fwrite(&m_center, sizeof(m_center), 1, o);
    fwrite(&m_radius, sizeof(m_radius), 1, o);
}
 
void Sphere::Serialize(float* array) const
{
    for (int i = 0; i < 3; i++)
    {
        array[i] = m_center[i];
    }
    array[3] = m_radius;
}
 
 
void Sphere::Transform(float scale, const Vec3f& translate)
{
    m_center *= scale;
    m_center += translate;
    m_radius *= scale;
}
 
SphereAsSquaresParametrization::SphereAsSquaresParametrization(const Sphere& sphere,
        const Vec3f& planeNormal)
    : m_sphere(sphere)
    , m_planeNormal(planeNormal)
    , m_hcs(GfxTL::Vector3Df(planeNormal))
{}
 
void SphereAsSquaresParametrization::Init(const Sphere& sphere,
        const Vec3f& planeNormal)
{
    m_sphere = sphere;
    m_planeNormal = planeNormal;
    m_hcs.FromNormal(planeNormal[0], planeNormal[1], planeNormal[2]);
}
 
float SphereAsSquaresParametrization::Parameters(const Vec3f& p,
        std::pair< float, float >* param) const
{
    // convert to hemisphere coordinates
    Vec3f s = p - m_sphere.Center();
    s.normalize();
    Vec3f hs;
    hs[0] = s.dot(m_hcs[0].Data());
    hs[1] = s.dot(m_hcs[1].Data());
    hs[2] = s.dot(m_planeNormal);
    float ret = hs[2];
    hs[2] = abs(hs[2]);
    std::pair< float, float > inDisk;
    Hemisphere2Disk(hs, &inDisk);
    Disk2Square(inDisk, param);
    return ret;
}
 
bool SphereAsSquaresParametrization::InSpace(
    const std::pair< float, float >& param, bool lower, Vec3f* p) const
{
    if (param.first < -0.1 || param.first > 1.1
        || param.second < -0.1 || param.second > 1.1)
    {
        return false;
    }
    std::pair< float, float > clampedParam;
    clampedParam.first = GfxTL::Math< float >::Clamp(param.first, 0, 1);
    clampedParam.second = GfxTL::Math< float >::Clamp(param.second, 0, 1);
    std::pair< float, float > inDisk;
    Square2Disk(clampedParam, &inDisk);
    Vec3f s;
    Disk2Hemisphere(inDisk, &s);
    *p = Vec3f((s[0] * m_hcs[0] + s[1] * m_hcs[1] +
                GfxTL::Vector3Df((lower ? -1 : 1) * s[2] * m_planeNormal)).Data());
    *p *= m_sphere.Radius();
    *p += m_sphere.Center();
    return true;
}
 
bool SphereAsSquaresParametrization::InSpace(
    const std::pair< float, float >& param, bool lower, Vec3f* p,
    Vec3f* n) const
{
    if (param.first < -0.1 || param.first > 1.1
        || param.second < -0.1 || param.second > 1.1)
    {
        return false;
    }
    std::pair< float, float > clampedParam;
    clampedParam.first = GfxTL::Math< float >::Clamp(param.first, 0, 1);
    clampedParam.second = GfxTL::Math< float >::Clamp(param.second, 0, 1);
    std::pair< float, float > inDisk;
    Square2Disk(clampedParam, &inDisk);
    Vec3f s;
    Disk2Hemisphere(inDisk, &s);
    if (lower)
    {
        s[2] *= -1;
    }
    *n = Vec3f((s[0] * m_hcs[0] + s[1] * m_hcs[1] +
                GfxTL::Vector3Df(s[2] * m_planeNormal)).Data());
    *p = m_sphere.Radius() * (*n);
    *p += m_sphere.Center();
    return true;
}
 
void SphereAsSquaresParametrization::Transform(
    const GfxTL::MatrixXX< 3, 3, float >& rot, const GfxTL::Vector3Df& trans)
{
    m_sphere = Sphere(Vec3f((rot * GfxTL::Vector3Df(m_sphere.Center())
                             + trans).Data()), m_sphere.Radius());
    m_planeNormal = Vec3f((rot * GfxTL::Vector3Df(m_planeNormal)).Data());
    m_hcs[0] = rot * m_hcs[0];
    m_hcs[1] = rot * m_hcs[1];
}
 
void SphereAsSquaresParametrization::HyperplaneCoordinateSystem(Vec3f* hcs0, Vec3f* hcs1, Vec3f* hcs2) const
{
    hcs0->setValue(m_hcs[0]);
    hcs1->setValue(m_hcs[1]);
    hcs2->setValue(m_hcs[2]);
}
 
 
void SphereAsSquaresParametrization::Hemisphere2Disk(const Vec3f& p,
        std::pair< float, float >* inDisk) const
{
    inDisk->first = std::sqrt(1 - p[2]);
    inDisk->second = std::atan2(p[1], p[0]);
}
 
void SphereAsSquaresParametrization::Disk2Square(
    const std::pair< float, float >& inDisk,
    std::pair< float, float >* inSquare) const
{
    float r = inDisk.first;
    float phi = inDisk.second;
    float a, b;
 
    if (phi < float(-M_PI / 4.0))
    {
        phi += float(2 * M_PI);
    }
 
    if (phi < float(M_PI / 4.0))
    {
        a = r;
        b = phi * a / float(M_PI / 4.0);
    }
    else if (phi < float(3 * M_PI / 4.0))
    {
        b = r;
        a = -(phi - float(M_PI / 2.0)) * b / float(M_PI / 4.0);
    }
    else if (phi < float(5 * M_PI / 4.0))
    {
        a = -r;
        b = (phi - float(M_PI)) * a / float(M_PI / 4.0);
    }
    else
    {
        b = -r;
        a = -(phi - float(3 * M_PI / 2.0)) * b / float(M_PI / 4.0);
    }
 
    inSquare->first = (a + float(1.0)) / float(2.0);
    inSquare->second = (b + float(1.0)) / float(2.0);
}
 
void SphereAsSquaresParametrization::Square2Disk(
    const std::pair< float, float >& inSquare,
    std::pair< float, float >* inDisk) const
{
    float phi, r;
    float a = 2 * inSquare.first - 1;
    float b = 2 * inSquare.second - 1;
 
    if (a > -b)
    {
        if (a > b)
        {
            r = a;
            phi = float(M_PI / 4.0) * (b / a);
        }
        else
        {
            r = b;
            phi = float(M_PI / 4.0) * (2 - (a / b));
        }
    }
    else
    {
        if (a < b)
        {
            r = -a;
            phi = float(M_PI / 4.0) * (4 + (b / a));
        }
        else
        {
            r = -b;
            if (b != 0)
            {
                phi = float(M_PI / 4.0) * (6 - (a / b));
            }
            else
            {
                phi = 0;
            }
        }
    }
 
    inDisk->first = r;
    inDisk->second = phi;
}
 
void SphereAsSquaresParametrization::Disk2Hemisphere(
    const std::pair< float, float >& inDisk, Vec3f* p) const
{
    (*p)[0] = inDisk.first * std::sqrt(2 - inDisk.first * inDisk.first)
              * std::cos(inDisk.second);
    (*p)[1] = inDisk.first * std::sqrt(2 - inDisk.first * inDisk.first)
              * std::sin(inDisk.second);
    (*p)[2] = 1 - inDisk.first * inDisk.first;
}