UT_CovarianceMatrix.h

Go to the documentation of this file.

/*
 * PROPRIETARY INFORMATION.  This software is proprietary to
 * Side Effects Software Inc., and is not to be reproduced,
 * transmitted, or disclosed in any way without written permission.
 *
 * COMMENTS:
 *      Covariance Matrix calculations for points, segments, and triangles
 *      in 2 and 3 dimensions. The point covariance is the usual one often
 *      computed in geometry processing. The segment and triangle versions
 *      integrate it over all the points of all the given segments or
 *      triangles.
 *
 *      The calculations are taken from the thesis of Stefan Gottschalk:
 *          "Collision Queries using Oriented Bounding Boxes",
 *      pages 52 and 57.
 *
 */

#ifndef __UT_CovarianceMatrix__
#define __UT_CovarianceMatrix__

#include "UT_Accumulator.h"
#include "UT_Array.h"
#include "UT_Matrix2.h"
#include "UT_SymMatrix3.h"
#include "UT_Vector2.h"
#include "UT_Vector3.h"

#include <SYS/SYS_Types.h>


namespace UT_CovarianceMatrix
{

// Calculate a 2x2 covariance matrix for a set of points in the plane.
// PT_POS must implemented a single parameter operator()(int i) to return
// a UT_Vector2T<T> for integer point number parameter values
// 0, ..., num_pts - 1.

template <typename T, typename PT_POS>
UT_Matrix2T<T>
pointCovariance2(int num_pts, PT_POS pt_pos,
                 UT_Vector2T<T> *calculated_centroid = nullptr)
{
    UT_Vector2T<T> our_centroid;
    auto &centroid = calculated_centroid ? *calculated_centroid : our_centroid;
    
    UT::Accumulator< UT_Vector2T<T> > ctr(UT_Vector2T<T>(T(0)));

    for (int i = 0, ie = num_pts; i < ie; i++)
        ctr += pt_pos(i);

    centroid = ctr.sum() / num_pts;

    UT::Accumulator<T> cxx(T(0)), cxy(T(0)), cyy(T(0));

    for (int i = 0, ie = num_pts; i < ie; i++)
    {
        auto p = pt_pos(i) - centroid;
        cxx += p.x() * p.x();
        cxy += p.x() * p.y();
        cyy += p.y() * p.y();
    }

    return { cxx.sum(), cxy.sum(), cxy.sum(), cyy.sum() };
}

// Calculate a 3x3 covariance matrix for a set of points in the space.
// PT_POS must implemented a single parameter operator()(int i) to return
// a UT_Vector3T<T> for integer point number parameter values
// 0, ..., num_pts - 1.

template <typename T, typename PT_POS>
UT_SymMatrix3T<T>
pointCovariance3(int num_pts, PT_POS pt_pos,
                 UT_Vector3T<T> *calculated_centroid = nullptr)
{
    UT_Vector3T<T> our_centroid;
    auto &centroid = calculated_centroid ? *calculated_centroid : our_centroid;
    
    UT::Accumulator< UT_Vector3T<T> > ctr(UT_Vector3T<T>(T(0)));

    for (int i = 0, ie = num_pts; i < ie; i++)
        ctr += pt_pos(i);

    centroid = ctr.sum() / num_pts;

    UT::Accumulator<T> cxx(T(0)), cxy(T(0)), cxz(T(0));
    UT::Accumulator<T> cyy(T(0)), cyz((0)), czz(T(0));

    for (int i = 0, ie = num_pts; i < ie; i++)
    {
        auto p = pt_pos(i) - centroid;

        cxx += p.x() * p.x();
        cxy += p.x() * p.y();
        cxz += p.x() * p.z();
        cyy += p.y() * p.y();
        cyz += p.y() * p.z();
        czz += p.z() * p.z();
    }

    return { cxx.sum(), cxy.sum(), cyy.sum(), cxz.sum(), cyz.sum(), czz.sum() };
}

// Calculate a 2x2 covariance matrix for a set of segments in the plane.
// PT_POS must implemented a single parameter operator()(int i) called
// with argument in range 0, ..., 2 * num_segs - 1.

template <typename T, typename PT_POS>
UT_Matrix2T<T>
segmentCovariance2(int num_segs, PT_POS seg_pt_pos,
                   UT_Vector2T<T> *calculated_centroid = nullptr,
                   T *calculated_length = nullptr)
{
    UT_Vector2T<T> our_centroid;
    auto &centroid = calculated_centroid ? *calculated_centroid : our_centroid;
      
    UT::Accumulator< UT_Vector2T<T> > ctr(UT_Vector2T<T>(T(0)));
    UT::Accumulator<T> len(T(0));

    UT_Array<T> seg_len;
    UT_Array< UT_Vector2T<T> > seg_centroid_x2;

    seg_len.setSizeNoInit(num_segs);
    seg_centroid_x2.setSizeNoInit(num_segs);

    for (int s = 0, se = num_segs; s < se; s++)
    {
        auto p = seg_pt_pos(2 * s);
        auto q = seg_pt_pos(2 * s + 1);

        seg_centroid_x2(s) = p + q;
        seg_len(s) = (q - p).length();
        len += seg_len(s);
        ctr += (seg_len(s) * seg_centroid_x2(s));
    }

    T l = len.sum();
    if (calculated_length)
        *calculated_length = l;
    
    auto centroid_x2 = ctr.sum() / l;

    if (l == 0.0)
        return { T(0), T(0), T(0), T(0) };

    // Centroid of the all triangles:
    centroid = T(0.5) * centroid_x2;

    UT::Accumulator<T> cxx(T(0)), cxy(T(0)), cyy(T(0));
    for (int s = 0, se = num_segs; s < se; s++)
    {
        auto p = seg_pt_pos(2 * s) - centroid;
        auto q = seg_pt_pos(2 * s + 1) - centroid;
        auto scx2 = seg_centroid_x2(s) - centroid_x2;

        auto sl = seg_len(s);

#ifdef STANDARD_SUM
        cxx += sl * (scx2.x() * scx2.x() + p.x() * p.x() + q.x() * q.x());
        cxy += sl * (scx2.x() * scx2.y() + p.x() * p.y() + q.x() * q.y());
        cyy += sl * (scx2.y() * scx2.y() + p.y() * p.y() + q.y() * q.y());
#else
        auto lp = sl * p;
        auto lq = sl * q;
        auto lscx2 = sl * scx2;

        cxx += lscx2.x() * scx2.x();
        cxx += lp.x() * p.x();
        cxx += lq.x() * q.x();

        cxy += lscx2.x() * scx2.y();
        cxy += lp.x() * p.y();
        cxy += lq.x() * q.y();

        cyy += lscx2.y() * scx2.y();
        cyy += lp.y() * p.y();
        cyy += lq.y() * q.y();
#endif
    }

    l *= T(6.0);

    return { cxx.sum() / l, cxy.sum() / l, cxy.sum() / l, cyy.sum() / l };
}

// Calculate a 3x3 covariance matrix for a set of segments in the space.
// PT_POS must implemented a single parameter operator()(int i) called
// with argument in range 0, ..., 2 * num_segs - 1.

template <typename T, typename PT_POS>
UT_SymMatrix3T<T>
segmentCovariance3(int num_segs, PT_POS seg_pt_pos,
                   UT_Vector3T<T> *calculated_centroid = nullptr,
                   T * calculated_length = nullptr)
{
    UT_Vector3T<T> our_centroid;
    auto &centroid = calculated_centroid ? *calculated_centroid : our_centroid;
    
    UT::Accumulator< UT_Vector3T<T> > ctr(UT_Vector3T<T>(T(0)));
    UT::Accumulator<T> len(T(0));

    UT_Array<T> seg_len;
    UT_Array< UT_Vector3T<T> > seg_centroid_x2;

    seg_len.setSizeNoInit(num_segs);
    seg_centroid_x2.setSizeNoInit(num_segs);

    for (int s = 0, se = num_segs; s < se; s++)
    {
        auto p = seg_pt_pos(2 * s);
        auto q = seg_pt_pos(2 * s + 1);

        seg_centroid_x2(s) = p + q;
        seg_len(s) = (q - p).length();
        len += seg_len(s);
        ctr += (seg_len(s) * seg_centroid_x2(s));
    }

    T l = len.sum();
    if (calculated_length)
        *calculated_length = l;
    
    if (l == T(0))
        return { T(0),  T(0),  T(0),  T(0),  T(0),  T(0) };

    auto centroid_x2 = ctr.sum() / l;

    // Centroid of the all triangles:
    centroid = T(0.5) * centroid_x2;

    UT::Accumulator<T> cxx(T(0)), cxy(T(0)), cxz(T(0));
    UT::Accumulator<T> cyy(T(0)), cyz((0)), czz(T(0));

    for (int s = 0, se = num_segs; s < se; s++)
    {
        auto p = seg_pt_pos(2 * s) - centroid;
        auto q = seg_pt_pos(2 * s + 1) - centroid;

        auto scx2 = seg_centroid_x2(s) - centroid_x2;
        auto sl = seg_len(s);

#ifdef STANDARD_SUM
        cxx += sl * (scx2.x() * scx2.x() + p.x() * p.x() + q.x() * q.x());
        cxy += sl * (scx2.x() * scx2.y() + p.x() * p.y() + q.x() * q.y());
        cxz += sl * (scx2.x() * scx2.z() + p.x() * p.z() + q.x() * q.z());
        cyy += sl * (scx2.y() * scx2.y() + p.y() * p.y() + q.y() * q.y());
        cyz += sl * (scx2.y() * scx2.z() + p.y() * p.z() + q.y() * q.z());
        czz += sl * (scx2.z() * scx2.z() + p.z() * p.z() + q.z() * q.z());
#else
        auto lp = sl * p;
        auto lq = sl * q;
        auto lscx2 = sl * scx2;

        cxx += lscx2.x() * scx2.x();
        cxx += lp.x() * p.x();
        cxx += lq.x() * q.x();

        cxy += lscx2.x() * scx2.y();
        cxy += lp.x() * p.y();
        cxy += lq.x() * q.y();

        cxy += lscx2.x() * scx2.z();
        cxy += lp.x() * p.z();
        cxy += lq.x() * q.z();

        cyy += lscx2.y() * scx2.y();
        cyy += lp.y() * p.y();
        cyy += lq.y() * q.y();

        cyz += lscx2.y() * scx2.z();
        cyz += lp.y() * p.z();
        cyz += lq.y() * q.z();

        czz += lscx2.z() * scx2.z();
        czz += lp.z() * p.z();
        czz += lq.z() * q.z();
#endif
    }

    l *= T(6.0);

    return { cxx.sum() / l, cxy.sum() / l, cyy.sum() / l,
             cxz.sum() / l, cyz.sum() / l, czz.sum() / l };
}

// Calculate a 2x2 covariance matrix for a set of triangles in the plane.
// PT_POS must implemented a single parameter operator()(int i) called
// with argument in range 0, ..., 3 * num_tris - 1.

template <typename T, typename PT_POS>
UT_Matrix2T<T>
triangleCovariance2(int num_tris, PT_POS tri_pt_pos,
                    UT_Vector2T<T> *calculated_centroid = nullptr,
                    T *calculated_area = nullptr)
{
    UT_Vector2T<T> our_centroid;
    auto &centroid = calculated_centroid ? *calculated_centroid : our_centroid;
    
    UT::Accumulator< UT_Vector2T<T> > ctr(UT_Vector2T<T>(T(0)));
    UT::Accumulator<T> area_x2(T(0));

    UT_Array<T> tri_area_x2;                            // triangle area x 2
    UT_Array< UT_Vector2T<T> > tri_centroid_x3;         // triangle centroid x 3

    tri_area_x2.setSizeNoInit(num_tris);
    tri_centroid_x3.setSizeNoInit(num_tris);

    for (int t = 0, te = num_tris; t < te; t++)
    {
        auto t0 = 3 * t;
        auto p = tri_pt_pos(t0);
        auto q = tri_pt_pos(t0 + 1);
        auto r = tri_pt_pos(t0 + 2);

        tri_centroid_x3(t) = (p + q) + r;
        tri_area_x2(t) = SYSabs((q.x() - p.x()) * (r.y() - p.y())
                                - (q.y() - p.y()) * (r.x() - p.x()));

        area_x2 += tri_area_x2(t);
        ctr += (tri_area_x2(t) * tri_centroid_x3(t));
    }

    auto a = area_x2.sum();
    if (calculated_area)
        *calculated_area = a * T(0.5);

    if (a == T(0))
        return { T(0), T(0), T(0), T(0) };

    auto centroid_x3 = ctr.sum() / a;

    // Centroid of the all triangles:
    centroid = centroid_x3 / 3.0;

    UT::Accumulator<T> cxx(T(0)), cxy(T(0)), cyy(T(0));

    for (int t = 0, te = num_tris; t < te; t++)
    {
        auto t0 = 3 * t;
        auto p = tri_pt_pos(t0) - centroid;
        auto q = tri_pt_pos(t0 + 1) - centroid;
        auto r = tri_pt_pos(t0 + 2) - centroid;

        auto tcx3 = tri_centroid_x3(t) - centroid_x3;
        auto tax2 = tri_area_x2(t);

#ifdef STANDARD_SUM
        cxx += tax2 * (tcx3.x() * tcx3.x()
                       + p.x() * p.x() + q.x() * q.x() + r.x() * r.x());
        cxy += tax2 * (tcx3.x() * tcx3.y()
                       + p.x() * p.y() + q.x() * q.y() + r.x() * r.y());
        cyy += tax2 * (tcx3.y() * tcx3.y()
                       + p.y() * p.y() + q.y() * q.y() + r.y() * r.y());
#else
        auto ap = tax2 * p;
        auto aq = tax2 * q;
        auto ar = tax2 * r;
        auto atcx3 = tax2 * tcx3;

        cxx += atcx3.x() * tcx3.x();
        cxx += ap.x() * p.x();
        cxx += aq.x() * q.x();
        cxx += ar.x() * r.x();

        cxy += atcx3.x() * tcx3.y();
        cxy += ap.x() * p.y();
        cxy += aq.x() * q.y();
        cxy += ar.x() * r.y();
        
        cyy += atcx3.y() * tcx3.y();
        cyy += ap.y() * p.y();
        cyy += aq.y() * q.y();
        cyy += ar.y() * r.y();
#endif
    }

    a *= T(12.0);

    return { cxx.sum() / a, cxy.sum() / a, cxy.sum() / a, cyy.sum() / a };
}

// Calculate a 3x3 covariance matrix for a set of triangles in the space.
// PT_POS must implemented a single parameter operator()(int i) called
// with argument in range 0, ..., 3 * num_tris - 1.

template <typename T, typename PT_POS>
UT_SymMatrix3T<T>
triangleCovariance3(int num_tris, PT_POS tri_pt_pos,
                    UT_Vector3T<T> *calculated_centroid = nullptr,
                    UT_Vector3T<T> *calculated_average_normal = nullptr,
                    T *calculated_area = nullptr)
{
    UT_Vector3T<T> our_centroid;
    auto &centroid = calculated_centroid ? *calculated_centroid : our_centroid;
    
    UT_Vector3T<T> our_average_normal;
    auto &average_normal =
        calculated_average_normal ? *calculated_average_normal
                                  : our_average_normal;
    
    UT::Accumulator< UT_Vector3T<T> > ctr(UT_Vector3T<T>(T(0)));
    UT::Accumulator< UT_Vector3T<T> > nml(UT_Vector3T<T>(T(0)));

    UT::Accumulator<T> area_x2(T(0));

    UT_Array<T> tri_area_x2;
    UT_Array< UT_Vector3T<T> > tri_centroid_x3;

    tri_area_x2.setSizeNoInit(num_tris);
    tri_centroid_x3.setSizeNoInit(num_tris);

    for (int t = 0, te = num_tris; t < te; t++)
    {
        auto t0 = 3 * t;
        auto p = tri_pt_pos(t0);
        auto q = tri_pt_pos(t0 + 1);
        auto r = tri_pt_pos(t0 + 2);

        tri_centroid_x3(t) = (p + q) + r;

        auto n = cross(q - p, r - p);

        // Add the cross product as unit normal weighted by (twice) tri area.
        nml += n;

        tri_area_x2(t) = n.length();
        area_x2 += tri_area_x2(t);

        ctr += (tri_area_x2(t) * tri_centroid_x3(t));
    }

    T a = area_x2.sum();
    
    if (calculated_area)
        *calculated_area = a * T(0.5);
    
    if (a == T(0))
        return {  T(0),  T(0),  T(0),  T(0),  T(0),  T(0) };

    auto centroid_x3 = ctr.sum() / a;

    // Centroid of the all triangles:
    centroid = centroid_x3 / 3.0;
    average_normal = nml.sum() / a;

    UT::Accumulator<T> cxx(T(0)), cxy(T(0)), cxz(T(0));
    UT::Accumulator<T> cyy(T(0)), cyz((0)), czz(T(0));

    for (int t = 0, te = num_tris; t < te; t++)
    {
        auto t0 = 3 * t;
        auto p = tri_pt_pos(t0) - centroid;
        auto q = tri_pt_pos(t0 + 1) - centroid;
        auto r = tri_pt_pos(t0 + 2) - centroid;

        auto tcx3 = tri_centroid_x3(t) - centroid_x3;
        auto tax2 = tri_area_x2(t);

#ifdef STANDARD_SUM
        cxx += tax2 * (tcx3.x() * tcx3.x()
                       + p.x() * p.x() + q.x() * q.x() + r.x() * r.x());
        cxy += tax2 * (tcx3.x() * tcx3.y()
                       + p.x() * p.y() + q.x() * q.y() + r.x() * r.y());
        cxz += tax2 * (tcx3.x() * tcx3.z()
                       + p.x() * p.z() + q.x() * q.z() + r.x() * r.z());
        cyy += tax2 * (tcx3.y() * tcx3.y()
                       + p.y() * p.y() + q.y() * q.y() + r.y() * r.y());
        cyz += tax2 * (tcx3.y() * tcx3.z()
                       + p.y() * p.z() + q.y() * q.z() + r.y() * r.z());
        czz += tax2 * (tcx3.z() * tcx3.z()
                       + p.z() * p.z() + q.z() * q.z() + r.z() * r.z());
#else

        auto ap = tax2 * p;
        auto aq = tax2 * q;
        auto ar = tax2 * r;
        auto atcx3 = tax2 * tcx3;

        cxx += atcx3.x() * tcx3.x();
        cxx += ap.x() * p.x();
        cxx += aq.x() * q.x();
        cxx += ar.x() * r.x();

        cxy += atcx3.x() * tcx3.y();
        cxy += ap.x() * p.y();
        cxy += aq.x() * q.y();
        cxy += ar.x() * r.y();
        
        cxz += atcx3.x() * tcx3.z();
        cxz += ap.x() * p.z();
        cxz += aq.x() * q.z();
        cxz += ar.x() * r.z();
        
        cyy += atcx3.y() * tcx3.y();
        cyy += ap.y() * p.y();
        cyy += aq.y() * q.y();
        cyy += ar.y() * r.y();
        
        cyz += atcx3.y() * tcx3.z();
        cyz += ap.y() * p.z();
        cyz += aq.y() * q.z();
        cyz += ar.y() * r.z();
        
        czz += atcx3.z() * tcx3.z();
        czz += ap.z() * p.z();
        czz += aq.z() * q.z();
        czz += ar.z() * r.z();

#endif
    }

    a *= T(12.0);

    return { cxx.sum() / a, cxy.sum() / a, cyy.sum() / a,
             cxz.sum() / a, cyz.sum() / a, czz.sum() / a };
}

} // namespace UT_CovarianceMatrix

#endif