/* ========================================================================= *
 *                                                                           *
 *                               OpenMesh                                    *
 *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
 *           Department of Computer Graphics and Multimedia                  *
 *                          All rights reserved.                             *
 *                            www.openmesh.org                               *
 *                                                                           *
 *---------------------------------------------------------------------------*
 * This file is part of OpenMesh.                                            *
 *---------------------------------------------------------------------------*
 *                                                                           *
 * Redistribution and use in source and binary forms, with or without        *
 * modification, are permitted provided that the following conditions        *
 * are met:                                                                  *
 *                                                                           *
 * 1. Redistributions of source code must retain the above copyright notice, *
 *    this list of conditions and the following disclaimer.                  *
 *                                                                           *
 * 2. Redistributions in binary form must reproduce the above copyright      *
 *    notice, this list of conditions and the following disclaimer in the    *
 *    documentation and/or other materials provided with the distribution.   *
 *                                                                           *
 * 3. Neither the name of the copyright holder nor the names of its          *
 *    contributors may be used to endorse or promote products derived from   *
 *    this software without specific prior written permission.               *
 *                                                                           *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
 *                                                                           *
 * ========================================================================= */

/*===========================================================================*\
 *                                                                           *
 *   $Revision$                                                         *
 *   $Date$                   *
 *                                                                           *
\*===========================================================================*/

/** \file Core/Geometry/QuadricT.hh

 */

//=============================================================================
//
//  CLASS QuadricT
//
//=============================================================================

#ifndef OPENMESH_GEOMETRY_QUADRICMETRIC_HH
    #define OPENMESH_GEOMETRY_QUADRICMETRIC_HH

    //== INCLUDES =================================================================
    #include <OpenMesh/Core/Geometry/VectorT.hh>
    #include <OpenMesh/Core/Utils/GenProg.hh>

//== NAMESPACE ================================================================

namespace OpenMesh {  // BEGIN_NS_OPENMESH
namespace Geometry {  // BEGIN_NS_GEOMETRY

//== CLASS DEFINITION =========================================================

/** /class QuadricT Geometry/QuadricT.hh

    Stores a quadric as a 4x4 symmetrix matrix. Used by the
    error quadric based mesh decimation algorithms.
**/

template<class Scalar>
class QuadricMetricT
{
  public:
    typedef Scalar value_type;
    typedef QuadricMetricT<Scalar> type;
    typedef QuadricMetricT<Scalar> Self;

    /// construct with upper triangle of symmetrix 4x4 matrix
    QuadricMetricT(Scalar _a, Scalar _b, Scalar _c, Scalar _d, Scalar _e, Scalar _f, Scalar _g, Scalar _h, Scalar _i, Scalar _j)
      : a_(_a),
        b_(_b),
        c_(_c),
        d_(_d),
        e_(_e),
        f_(_f),
        g_(_g),
        h_(_h),
        i_(_i),
        j_(_j)
    {
        count = 1;
    }

    /// constructor from given plane equation: ax+by+cz+d_=0
    QuadricMetricT(Scalar _a = 0.0, Scalar _b = 0.0, Scalar _c = 0.0, Scalar _d = 0.0)
      : a_(_a * _a),
        b_(_a * _b),
        c_(_a * _c),
        d_(_a * _d),
        e_(_b * _b),
        f_(_b * _c),
        g_(_b * _d),
        h_(_c * _c),
        i_(_c * _d),
        j_(_d * _d)
    {
        count = 1;
    }

    template<class _Point>
    QuadricMetricT(const _Point& _pt)
    {
        set_distance_to_point(_pt);
    }

    template<class _Normal, class _Point>
    QuadricMetricT(const _Normal& _n, const _Point& _p)
    {
        set_distance_to_plane(_n, _p);
    }

    // set operator
    void set(Scalar _a, Scalar _b, Scalar _c, Scalar _d, Scalar _e, Scalar _f, Scalar _g, Scalar _h, Scalar _i, Scalar _j)
    {
        a_ = _a;
        b_ = _b;
        c_ = _c;
        d_ = _d;
        e_ = _e;
        f_ = _f;
        g_ = _g;
        h_ = _h;
        i_ = _i;
        j_ = _j;
    }

    // sets the quadric representing the squared distance to _pt
    template<class _Point>
    void set_distance_to_point(const _Point& _pt)
    {
        set(1, 0, 0, -_pt[0], 1, 0, -_pt[1], 1, -_pt[2], dot(_pt, _pt));
    }

    // sets the quadric representing the squared distance to the plane [_a,_b,_c,_d]
    void set_distance_to_plane(Scalar _a, Scalar _b, Scalar _c, Scalar _d)
    {
        a_ = _a * _a;
        b_ = _a * _b;
        c_ = _a * _c;
        d_ = _a * _d;
        e_ = _b * _b;
        f_ = _b * _c;
        g_ = _b * _d;
        h_ = _c * _c;
        i_ = _c * _d;
        j_ = _d * _d;
    }

    // sets the quadric representing the squared distance to the plane
    // determined by the normal _n and the point _p
    template<class _Normal, class _Point>
    void set_distance_to_plane(const _Normal& _n, const _Point& _p)
    {
        set_distance_to_plane(_n[0], _n[1], _n[2], -dot(_n, _p));
    }

    /// set all entries to zero
    void clear()
    {
        a_ = b_ = c_ = d_ = e_ = f_ = g_ = h_ = i_ = j_ = 0.0;
        count = 0;
    }

    /// add quadrics
    QuadricMetricT<Scalar>& operator+=(const QuadricMetricT<Scalar>& _q)
    {
        a_ += _q.a_;
        b_ += _q.b_;
        c_ += _q.c_;
        d_ += _q.d_;
        e_ += _q.e_;
        f_ += _q.f_;
        g_ += _q.g_;
        h_ += _q.h_;
        i_ += _q.i_;
        j_ += _q.j_;
        count += _q.count;
        return *this;
    }

    QuadricMetricT<Scalar> operator+(const QuadricMetricT<Scalar>& _other) const
    {
        QuadricMetricT<Scalar> result = *this;
        return result += _other;
    }

    /// multiply by scalar
    QuadricMetricT<Scalar>& operator*=(Scalar _s)
    {
        a_ *= _s;
        b_ *= _s;
        c_ *= _s;
        d_ *= _s;
        e_ *= _s;
        f_ *= _s;
        g_ *= _s;
        h_ *= _s;
        i_ *= _s;
        j_ *= _s;
        return *this;
    }

    QuadricMetricT<Scalar> operator*(Scalar _s) const
    {
        QuadricMetricT<Scalar> result = *this;
        return result *= _s;
    }

    /// multiply 4D vector from right: Q*v
    template<class _Vec4>
    _Vec4 operator*(const _Vec4& _v) const
    {
        Scalar x(_v[0]), y(_v[1]), z(_v[2]), w(_v[3]);
        return _Vec4(
          x * a_ + y * b_ + z * c_ + w * d_, x * b_ + y * e_ + z * f_ + w * g_, x * c_ + y * f_ + z * h_ + w * i_, x * d_ + y * g_ + z * i_ + w * j_);
    }

    /// evaluate quadric Q at (3D or 4D) vector v: v*Q*v
    template<class _Vec>
    Scalar operator()(const _Vec& _v) const
    {
        return evaluate(_v, GenProg::Int2Type<_Vec::size_>());
    }

    Scalar a() const { return a_; }
    Scalar b() const { return b_; }
    Scalar c() const { return c_; }
    Scalar d() const { return d_; }
    Scalar e() const { return e_; }
    Scalar f() const { return f_; }
    Scalar g() const { return g_; }
    Scalar h() const { return h_; }
    Scalar i() const { return i_; }
    Scalar j() const { return j_; }

    Scalar xx() const { return a_; }
    Scalar xy() const { return b_; }
    Scalar xz() const { return c_; }
    Scalar xw() const { return d_; }
    Scalar yy() const { return e_; }
    Scalar yz() const { return f_; }
    Scalar yw() const { return g_; }
    Scalar zz() const { return h_; }
    Scalar zw() const { return i_; }
    Scalar ww() const { return j_; }

    unsigned int getCount() const { return count; }

  protected:
    /// evaluate quadric Q at 3D vector v: v*Q*v
    template<class _Vec3>
    Scalar evaluate(const _Vec3& _v, GenProg::Int2Type<3> /*_dimension*/) const
    {
        Scalar x(_v[0]), y(_v[1]), z(_v[2]);
        return a_ * x * x + 2.0 * b_ * x * y + 2.0 * c_ * x * z + 2.0 * d_ * x + e_ * y * y + 2.0 * f_ * y * z + 2.0 * g_ * y + h_ * z * z +
               2.0 * i_ * z + j_;
    }

    /// evaluate quadric Q at 4D vector v: v*Q*v
    template<class _Vec4>
    Scalar evaluate(const _Vec4& _v, GenProg::Int2Type<4> /*_dimension*/) const
    {
        Scalar x(_v[0]), y(_v[1]), z(_v[2]), w(_v[3]);
        return a_ * x * x + 2.0 * b_ * x * y + 2.0 * c_ * x * z + 2.0 * d_ * x * w + e_ * y * y + 2.0 * f_ * y * z + 2.0 * g_ * y * w + h_ * z * z +
               2.0 * i_ * z * w + j_ * w * w;
    }

  private:
    Scalar a_, b_, c_, d_, e_, f_, g_, h_, i_, j_;

    unsigned int count;
};

/// Quadric using floats
typedef QuadricMetricT<float> QuadricMetricTf;

/// Quadric using double
typedef QuadricMetricT<double> QuadricMetricd;

//=============================================================================
}  // namespace Geometry
}  // namespace OpenMesh
//============================================================================
#endif  // OPENMESH_GEOMETRY_HH defined
//=============================================================================
