Matrix.h

Go to the documentation of this file.

//     ____   ______ __
//    / __ \ / ____// /
//   / /_/ // /    / /
//  / ____// /___ / /___   PixInsight Class Library
// /_/     \____//_____/   PCL 2.7.0
// ----------------------------------------------------------------------------
// pcl/Matrix.h - Released 2024-06-18T15:48:54Z
// ----------------------------------------------------------------------------
// This file is part of the PixInsight Class Library (PCL).
// PCL is a multiplatform C++ framework for development of PixInsight modules.
//
// Copyright (c) 2003-2024 Pleiades Astrophoto S.L. All Rights Reserved.
//
// Redistribution and use in both source and binary forms, with or without
// modification, is permitted provided that the following conditions are met:
//
// 1. All redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. All 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 names "PixInsight" and "Pleiades Astrophoto", nor the names
//    of their contributors, may be used to endorse or promote products derived
//    from this software without specific prior written permission. For written
//    permission, please contact info@pixinsight.com.
//
// 4. All products derived from this software, in any form whatsoever, must
//    reproduce the following acknowledgment in the end-user documentation
//    and/or other materials provided with the product:
//
//    "This product is based on software from the PixInsight project, developed
//    by Pleiades Astrophoto and its contributors (https://pixinsight.com/)."
//
//    Alternatively, if that is where third-party acknowledgments normally
//    appear, this acknowledgment must be reproduced in the product itself.
//
// THIS SOFTWARE IS PROVIDED BY PLEIADES ASTROPHOTO AND ITS 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 PLEIADES ASTROPHOTO OR ITS
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, BUSINESS
// INTERRUPTION; PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; AND LOSS OF USE,
// DATA OR PROFITS) 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.
// ----------------------------------------------------------------------------
 
#ifndef __PCL_Matrix_h
#define __PCL_Matrix_h
 
 
#include <pcl/Defs.h>
#include <pcl/Diagnostics.h>
 
#include <pcl/Container.h>
#include <pcl/Exception.h>
#include <pcl/Math.h>
#include <pcl/Memory.h>
#include <pcl/ReferenceCounter.h>
#include <pcl/Rotate.h> // pcl::Reverse()
#include <pcl/Utility.h>
#include <pcl/Vector.h>
 
#ifndef __PCL_NO_MATRIX_STATISTICS
#  include <pcl/Selection.h>
#  include <pcl/Sort.h>
#endif
 
#if !defined( __PCL_NO_MATRIX_IMAGE_RENDERING ) && !defined( __PCL_NO_MATRIX_IMAGE_CONVERSION )
#  include <pcl/Image.h>
#  include <pcl/ImageVariant.h>
#endif
 
#if !defined( __PCL_NO_MATRIX_PHASE_MATRICES ) && !defined( __PCL_NO_VECTOR_INSTANTIATE )
#  include <pcl/Complex.h>
#endif
 
/*
 * Valid filter kernel sizes are odd integers >= 3. This macro is used for the
 * KernelFilter and SeparableFilter classes to ensure validity of filter
 * matrices and vectors.
 */
#define PCL_VALID_KERNEL_SIZE( n )  (n ? Max( 3, n|1 ) : 0)
 
namespace pcl
{
 
// ----------------------------------------------------------------------------
 
template <typename T>
class PCL_CLASS GenericMatrix : public DirectContainer<T>
{
public:
 
   using element = T;
 
   using vector = GenericVector<element>;
 
   using block_iterator = element*;
 
   using const_block_iterator = const element*;
 
   GenericMatrix()
   {
      m_data = new Data;
   }
 
   GenericMatrix( int rows, int cols )
   {
      PCL_PRECONDITION( rows >= 0 && cols >= 0 )
      m_data = new Data( rows, cols );
   }
 
   GenericMatrix( const element& x, int rows, int cols )
   {
      PCL_PRECONDITION( rows >= 0 && cols >= 0 )
      m_data = new Data( rows, cols );
      pcl::Fill( m_data->Begin(), m_data->End(), x );
   }
 
   template <typename T1>
   GenericMatrix( const T1* a, int rows, int cols )
   {
      PCL_PRECONDITION( rows >= 0 && cols >= 0 )
      m_data = new Data( rows, cols );
      if ( a != nullptr )
      {
               block_iterator __restrict__ i = m_data->Begin();
         const_block_iterator __restrict__ j = m_data->End();
         const T1*            __restrict__ k = a;
         PCL_IVDEP
         for ( ; i < j; ++i, ++k )
            *i = element( *k );
      }
   }
 
   template <typename T1>
   GenericMatrix( const T1& a00, const T1& a01,
                  const T1& a10, const T1& a11 )
   {
      m_data = new Data( 2, 2 );
      block_iterator __restrict__ v = m_data->Begin();
      v[0] = element( a00 ); v[1] = element( a01 );
      v[2] = element( a10 ); v[3] = element( a11 );
   }
 
   template <typename T1>
   GenericMatrix( const T1& a00, const T1& a01, const T1& a02,
                  const T1& a10, const T1& a11, const T1& a12,
                  const T1& a20, const T1& a21, const T1& a22 )
   {
      m_data = new Data( 3, 3 );
      block_iterator __restrict__ v = m_data->Begin();
      v[0] = element( a00 ); v[1] = element( a01 ); v[2] = element( a02 );
      v[3] = element( a10 ); v[4] = element( a11 ); v[5] = element( a12 );
      v[6] = element( a20 ); v[7] = element( a21 ); v[8] = element( a22 );
   }
 
   GenericMatrix( const GenericMatrix& x )
      : m_data( x.m_data )
   {
      m_data->Attach();
   }
 
   GenericMatrix( GenericMatrix&& x )
      : m_data( x.m_data )
   {
      x.m_data = nullptr;
   }
 
   GenericMatrix( const GenericMatrix& x, int i0, int j0, int rows, int cols )
   {
      i0 = Range( i0, 0, Max( 0, x.Rows()-1 ) );
      j0 = Range( j0, 0, Max( 0, x.Cols()-1 ) );
      rows = Range( rows, 0, Max( 0, x.Rows()-i0 ) );
      cols = Range( cols, 0, Max( 0, x.Cols()-j0 ) );
      m_data = new Data( rows, cols );
      for ( int i = 0; i < m_data->Rows(); ++i, ++i0 )
         for ( int j = 0; j < m_data->Cols(); ++j, ++j0 )
            m_data->v[i][j] = x.m_data->v[i0][j0];
   }
 
#ifndef __PCL_NO_MATRIX_IMAGE_CONVERSION
 
   template <class P>
   GenericMatrix( const GenericImage<P>& image, const Rect& rect = Rect( 0 ), int channel = -1 )
   {
      GenericMatrix M = FromImage( image, rect, channel );
      pcl::Swap( m_data, M.m_data );
   }
 
   GenericMatrix( const ImageVariant& image, const Rect& rect = Rect( 0 ), int channel = -1 )
   {
      GenericMatrix M = FromImage( image, rect, channel );
      pcl::Swap( m_data, M.m_data );
   }
 
#endif   // !__PCL_NO_MATRIX_IMAGE_CONVERSION
 
   virtual ~GenericMatrix()
   {
      if ( m_data != nullptr )
      {
         DetachFromData();
         m_data = nullptr;
      }
   }
 
   void Clear()
   {
      if ( !IsEmpty() )
         if ( m_data->IsUnique() )
            m_data->Deallocate();
         else
         {
            Data* newData = new Data( 0, 0 );
            DetachFromData();
            m_data = newData;
         }
   }
 
   GenericMatrix& operator =( const GenericMatrix& x )
   {
      Assign( x );
      return *this;
   }
 
   void Assign( const GenericMatrix& x )
   {
      x.m_data->Attach();
      DetachFromData();
      m_data = x.m_data;
   }
 
   GenericMatrix& operator =( GenericMatrix&& x )
   {
      Transfer( x );
      return *this;
   }
 
   void Transfer( GenericMatrix& x )
   {
      DetachFromData();
      m_data = x.m_data;
      x.m_data = nullptr;
   }
 
   void Transfer( GenericMatrix&& x )
   {
      DetachFromData();
      m_data = x.m_data;
      x.m_data = nullptr;
   }
 
   friend void Swap( GenericMatrix& x1, GenericMatrix& x2 ) noexcept
   {
      pcl::Swap( x1.m_data, x2.m_data );
   }
 
   GenericMatrix& operator =( const element& x )
   {
      if ( !IsUnique() )
      {
         Data* newData = new Data( m_data->Rows(), m_data->Cols() );
         DetachFromData();
         m_data = newData;
      }
      pcl::Fill( m_data->Begin(), m_data->End(), x );
      return *this;
   }
 
#define IMPLEMENT_SCALAR_ASSIGN_OP( op )                                      \
      if ( IsUnique() )                                                       \
      {                                                                       \
               block_iterator __restrict__ i = m_data->Begin();               \
         const_block_iterator __restrict__ j = m_data->End();                 \
         PCL_IVDEP                                                            \
         for ( ; i < j; ++i )                                                 \
            *i op##= x;                                                       \
      }                                                                       \
      else                                                                    \
      {                                                                       \
         Data* newData = new Data( m_data->Rows(), m_data->Cols() );          \
               block_iterator __restrict__ i = newData->Begin();              \
         const_block_iterator __restrict__ j = newData->End();                \
         const_block_iterator __restrict__ k = m_data->Begin();               \
         PCL_IVDEP                                                            \
         for ( ; i < j; ++i, ++k )                                            \
            *i = *k op x;                                                     \
         DetachFromData();                                                    \
         m_data = newData;                                                    \
      }                                                                       \
      return *this;
 
   GenericMatrix& operator +=( const element& x )
   {
      IMPLEMENT_SCALAR_ASSIGN_OP( + )
   }
 
   GenericMatrix& operator -=( const element& x )
   {
      IMPLEMENT_SCALAR_ASSIGN_OP( - )
   }
 
   GenericMatrix& operator *=( const element& x )
   {
      IMPLEMENT_SCALAR_ASSIGN_OP( * )
   }
 
   GenericMatrix& operator /=( const element& x )
   {
      IMPLEMENT_SCALAR_ASSIGN_OP( / )
   }
 
   GenericMatrix& operator ^=( const element& x )
   {
      if ( IsUnique() )
      {
               block_iterator __restrict__ i = m_data->Begin();
         const_block_iterator __restrict__ j = m_data->End();
         PCL_IVDEP
         for ( ; i < j; ++i )
            *i = pcl::Pow( *i, x );
      }
      else
      {
         Data* newData = new Data( m_data->Rows(), m_data->Cols() );
               block_iterator __restrict__ i = newData->Begin();
         const_block_iterator __restrict__ j = newData->End();
         const_block_iterator __restrict__ k = m_data->Begin();
         PCL_IVDEP
         for ( ; i < j; ++i, ++k )
            *i = pcl::Pow( *k, x );
         DetachFromData();
         m_data = newData;
      }
      return *this;
   }
 
#undef IMPLEMENT_SCALAR_ASSIGN_OP
 
#ifndef __PCL_NO_MATRIX_ELEMENT_WISE_ARITHMETIC_OPERATIONS
 
#define IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( op )                                                        \
      if ( IsUnique() )                                                                               \
      {                                                                                               \
               block_iterator __restrict__ i = m_data->Begin();                                       \
         const_block_iterator __restrict__ j = pcl::Min( m_data->End(),                               \
                                                         m_data->Begin() + x.NumberOfElements() );    \
         const_block_iterator __restrict__ k = x.m_data->Begin();                                     \
         PCL_IVDEP                                                                                    \
         for ( ; i < j; ++i, ++k )                                                                    \
            *i op##= *k;                                                                              \
      }                                                                                               \
      else                                                                                            \
      {                                                                                               \
         Data* newData = new Data( m_data->Rows(), m_data->Cols() );                                  \
               block_iterator __restrict__ i = newData->Begin();                                      \
         const_block_iterator __restrict__ j = pcl::Min( newData->End(),                              \
                                                         newData->Begin() + x.NumberOfElements() );   \
         const_block_iterator __restrict__ k = x.m_data->Begin();                                     \
         const_block_iterator __restrict__ t = m_data->Begin();                                       \
         PCL_IVDEP                                                                                    \
         for ( ; i < j; ++i, ++k, ++t )                                                               \
            *i = *t op *k;                                                                            \
         DetachFromData();                                                                            \
         m_data = newData;                                                                            \
      }                                                                                               \
      return *this;
 
   GenericMatrix& operator +=( const GenericMatrix& x )
   {
      IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( + )
   }
 
   GenericMatrix& operator -=( const GenericMatrix& x )
   {
      IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( - )
   }
 
   GenericMatrix& operator *=( const GenericMatrix& x )
   {
      IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( * )
   }
 
   GenericMatrix& operator /=( const GenericMatrix& x )
   {
      IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( / )
   }
 
   GenericMatrix& operator ^=( const GenericMatrix& x )
   {
      if ( IsUnique() )
      {
               block_iterator __restrict__ i = m_data->Begin();
         const_block_iterator __restrict__ j = pcl::Min( m_data->End(),
                                                         m_data->Begin() + x.NumberOfElements() );
         const_block_iterator __restrict__ k = x.m_data->Begin();
         PCL_IVDEP
         for ( ; i < j; ++i, ++k )
            *i = pcl::Pow( *i, *k );
      }
      else
      {
         Data* newData = new Data( m_data->Rows(), m_data->Cols() );
               block_iterator __restrict__ i = newData->Begin();
         const_block_iterator __restrict__ j = pcl::Min( newData->End(),
                                                         newData->Begin() + x.NumberOfElements() );
         const_block_iterator __restrict__ k = x.m_data->Begin();
         const_block_iterator __restrict__ t = m_data->Begin();
         PCL_IVDEP
         for ( ; i < j; ++i, ++k, ++t )
            *i = pcl::Pow( *t, *k );
         DetachFromData();
         m_data = newData;
      }
      return *this;
   }
 
#undef IMPLEMENT_ELEMENT_WISE_ASSIGN_OP
 
#endif   // __PCL_NO_MATRIX_ELEMENT_WISE_ARITHMETIC_OPERATIONS
 
   GenericMatrix Sqr() const
   {
      GenericMatrix R( m_data->Rows(), m_data->Cols() );
            block_iterator __restrict__ i = R.m_data->Begin();
      const_block_iterator __restrict__ j = R.m_data->End();
      const_block_iterator __restrict__ k = m_data->Begin();
      PCL_IVDEP
      for ( ; i < j; ++i, ++k )
         *i = *k * *k;
      return R;
   }
 
   void SetSqr()
   {
      if ( IsUnique() )
      {
               block_iterator __restrict__ i = m_data->Begin();
         const_block_iterator __restrict__ j = m_data->End();
         PCL_IVDEP
         for ( ; i < j; ++i )
            *i *= *i;
      }
      else
      {
         Data* newData = new Data( m_data->Rows(), m_data->Cols() );
               block_iterator __restrict__ i = newData->Begin();
         const_block_iterator __restrict__ j = newData->End();
         const_block_iterator __restrict__ k = m_data->Begin();
         PCL_IVDEP
         for ( ; i < j; ++i, ++k )
            *i = *k * *k;
         DetachFromData();
         m_data = newData;
      }
   }
 
   GenericMatrix Sqrt() const
   {
      GenericMatrix R( m_data->Rows(), m_data->Cols() );
            block_iterator __restrict__ i = R.m_data->Begin();
      const_block_iterator __restrict__ j = R.m_data->End();
      const_block_iterator __restrict__ k = m_data->Begin();
      PCL_IVDEP
      for ( ; i < j; ++i, ++k )
         *i = pcl::Sqrt( *k );
      return R;
   }
 
   void SetSqrt()
   {
      if ( IsUnique() )
      {
               block_iterator __restrict__ i = m_data->Begin();
         const_block_iterator __restrict__ j = m_data->End();
         PCL_IVDEP
         for ( ; i < j; ++i )
            *i = pcl::Sqrt( *i );
      }
      else
      {
         Data* newData = new Data( m_data->Rows(), m_data->Cols() );
               block_iterator __restrict__ i = newData->Begin();
         const_block_iterator __restrict__ j = newData->End();
         const_block_iterator __restrict__ k = m_data->Begin();
         PCL_IVDEP
         for ( ; i < j; ++i, ++k )
            *i = pcl::Sqrt( *k );
         DetachFromData();
         m_data = newData;
      }
   }
 
   GenericMatrix Abs() const
   {
      GenericMatrix R( m_data->Rows(), m_data->Cols() );
            block_iterator __restrict__ i = R.m_data->Begin();
      const_block_iterator __restrict__ j = R.m_data->End();
      const_block_iterator __restrict__ k = m_data->Begin();
      PCL_IVDEP
      for ( ; i < j; ++i, ++k )
         *i = pcl::Abs( *k );
      return R;
   }
 
   void SetAbs()
   {
      if ( IsUnique() )
      {
               block_iterator __restrict__ i = m_data->Begin();
         const_block_iterator __restrict__ j = m_data->End();
         PCL_IVDEP
         for ( ; i < j; ++i )
            *i = pcl::Abs( *i );
      }
      else
      {
         Data* newData = new Data( m_data->Rows(), m_data->Cols() );
               block_iterator __restrict__ i = newData->Begin();
         const_block_iterator __restrict__ j = newData->End();
         const_block_iterator __restrict__ k = m_data->Begin();
         PCL_IVDEP
         for ( ; i < j; ++i, ++k )
            *i = pcl::Abs( *k );
         DetachFromData();
         m_data = newData;
      }
   }
 
   bool IsUnique() const noexcept
   {
      return m_data->IsUnique();
   }
 
   bool IsAliasOf( const GenericMatrix& x ) const noexcept
   {
      return m_data == x.m_data;
   }
 
   void EnsureUnique()
   {
      if ( !IsUnique() )
      {
         Data* newData = new Data( m_data->Rows(), m_data->Cols() );
         const_block_iterator __restrict__ i = m_data->Begin();
         const_block_iterator __restrict__ j = m_data->End();
               block_iterator __restrict__ k = newData->Begin();
         PCL_IVDEP
         for ( ; i < j; ++i, ++k )
            *k = *i;
         DetachFromData();
         m_data = newData;
      }
   }
 
   int Rows() const noexcept
   {
      return m_data->Rows();
   }
 
   int Cols() const noexcept
   {
      return m_data->Cols();
   }
 
   int Columns() const noexcept
   {
      return Cols();
   }
 
   bool IsValid() const noexcept
   {
      return m_data != nullptr;
   }
 
   bool IsEmpty() const noexcept
   {
      return Rows() == 0 || Cols() == 0;
   }
 
   operator bool() const noexcept
   {
      return !IsEmpty();
   }
 
   size_type NumberOfElements() const noexcept
   {
      return m_data->NumberOfElements();
   }
 
   size_type Size() const noexcept
   {
      return m_data->Size();
   }
 
   bool operator ==( const GenericMatrix& x ) const noexcept
   {
      return IsAliasOf( x ) || SameDimensions( x ) && pcl::Equal( Begin(), x.Begin(), x.End() );
   }
 
   bool operator <( const GenericMatrix& x ) const noexcept
   {
      return !IsAliasOf( x ) && pcl::Compare( Begin(), End(), x.Begin(), x.End() ) < 0;
   }
 
   bool SameDimensions( const GenericMatrix& x ) const noexcept
   {
      return Rows() == x.Rows() && Cols() == x.Cols();
   }
 
   bool SameElements( const GenericMatrix& x ) const noexcept
   {
      if ( unlikely( IsAliasOf( x ) ) )
         return true;
      if ( unlikely( NumberOfElements() != x.NumberOfElements() ) )
         return false;
      const_block_iterator __restrict__ i = Begin();
      const_block_iterator __restrict__ j = End();
      const_block_iterator __restrict__ k = x.Begin();
      PCL_IVDEP
      for ( ; i < j; ++i, ++k )
         if ( *i != *k )
            return false;
      return true;
   }
 
   element& Element( int i, int j )
   {
      EnsureUnique();
      return m_data->Element( i, j );
   }
 
   const element& Element( int i, int j ) const noexcept
   {
      return m_data->Element( i, j );
   }
 
   block_iterator operator []( int i )
   {
      EnsureUnique();
      return m_data->v[i];
   }
 
   const_block_iterator operator []( int i ) const noexcept
   {
      return m_data->v[i];
   }
 
   block_iterator Begin()
   {
      EnsureUnique();
      return m_data->Begin();
   }
 
   const_block_iterator Begin() const noexcept
   {
      return m_data->Begin();
   }
 
   const_block_iterator ConstBegin() const noexcept
   {
      return Begin();
   }
 
   block_iterator operator *()
   {
      return Begin();
   }
 
   const_block_iterator operator *() const noexcept
   {
      return Begin();
   }
 
   block_iterator End()
   {
      EnsureUnique();
      return m_data->End();
   }
 
   const_block_iterator End() const noexcept
   {
      return m_data->End();
   }
 
   const_block_iterator ConstEnd() const noexcept
   {
      return End();
   }
 
   block_iterator* DataPtr() noexcept
   {
      return m_data->v;
   }
 
   block_iterator RowPtr( int i ) noexcept
   {
      return m_data->v[i];
   }
 
#ifndef __PCL_NO_STL_COMPATIBLE_ITERATORS
   block_iterator begin()
   {
      return Begin();
   }
 
   const_block_iterator begin() const noexcept
   {
      return Begin();
   }
 
   block_iterator end()
   {
      return End();
   }
 
   const_block_iterator end() const noexcept
   {
      return End();
   }
#endif   // !__PCL_NO_STL_COMPATIBLE_ITERATORS
 
   vector RowVector( int i ) const
   {
      vector r( m_data->Cols() );
      for ( int j = 0; j < m_data->Cols(); ++j )
         r[j] = m_data->v[i][j];
      return r;
   }
 
   vector ColumnVector( int j ) const
   {
      vector c( m_data->Rows() );
      for ( int i = 0; i < m_data->Rows(); ++i )
         c[i] = m_data->v[i][j];
      return c;
   }
 
   vector ColVector( int j ) const
   {
      return ColumnVector( j );
   }
 
   template <class V>
   void SetRow( int i, const V& r )
   {
      EnsureUnique();
      for ( int j = 0; j < m_data->Cols() && j < r.Length(); ++j )
         m_data->v[i][j] = element( r[j] );
   }
 
   template <class V>
   void SetColumn( int j, const V& c )
   {
      EnsureUnique();
      for ( int i = 0; i < m_data->Rows() && i < c.Length(); ++i )
         m_data->v[i][j] = element( c[i] );
   }
 
   template <class V>
   void SetCol( int j, const V& c )
   {
      SetColumn( j, c );
   }
 
   static GenericMatrix FromRowVector( const vector& r )
   {
      GenericMatrix R( element( 0 ), r.Length(), r.Length() );
      for ( int j = 0; j < r.Length(); ++j )
         R.m_data->v[0][j] = r[j];
      return R;
   }
 
   static GenericMatrix FromColumnVector( const vector& c )
   {
      GenericMatrix C( element( 0 ), c.Length(), c.Length() );
      for ( int i = 0; i < c.Length(); ++i )
         C.m_data->v[i][0] = c[i];
      return C;
   }
 
   static GenericMatrix FromColVector( const vector& c )
   {
      return FromColumnVector( c );
   }
 
   static GenericMatrix UnitMatrix( int n )
   {
      GenericMatrix One( element( 0 ), n, n );
      for ( int i = 0; i < n; ++i )
         One[i][i] = element( 1 );
      return One;
   }
 
   GenericMatrix Transpose() const
   {
      GenericMatrix Tr( m_data->Cols(), m_data->Rows() );
      for ( int i = 0; i < m_data->Rows(); ++i )
         for ( int j = 0; j < m_data->Cols(); ++j )
            Tr.m_data->v[j][i] = m_data->v[i][j];
      return Tr;
   }
 
   GenericMatrix Inverse() const;
 
   void Invert();
 
   void Flip()
   {
      EnsureUnique();
      pcl::Reverse( m_data->Begin(), m_data->End() );
   }
 
   GenericMatrix Flipped() const
   {
      GenericMatrix Tf( m_data->Cols(), m_data->Rows() );
      pcl::CopyReversed( Tf.m_data->End(), m_data->Begin(), m_data->End() );
      return Tf;
   }
 
   static GenericMatrix Translation( double dx, double dy )
   {
      return GenericMatrix( element( 1 ), element( 0 ), element( dx ),
                            element( 0 ), element( 1 ), element( dy ),
                            element( 0 ), element( 0 ), element( 1  ) );
   }
 
   template <class V>
   static GenericMatrix Translation( const V& delta )
   {
      return GenericMatrix( element( 1 ), element( 0 ), element( double( delta[0] ) ),
                            element( 0 ), element( 1 ), element( double( delta[1] ) ),
                            element( 0 ), element( 0 ), element( 1                ) );
   }
 
   void Translate( double dx, double dy )
   {
      PCL_PRECONDITION( Rows() == 3 && Cols() == 3 )
      EnsureUnique();
      block_iterator __restrict__ A0 = m_data->v[0];
      block_iterator __restrict__ A1 = m_data->v[1];
      block_iterator __restrict__ A2 = m_data->v[2];
      A0[2] = element( A0[2] + A0[0]*dx + A0[1]*dy );
      A1[2] = element( A1[2] + A1[0]*dx + A1[1]*dy );
      A2[2] = element( A2[2] + A2[0]*dx + A2[1]*dy );
   }
 
   template <class V>
   void Translate( const V& delta )
   {
      Translate( double( delta[0] ), double( delta[1] ) );
   }
 
   GenericMatrix Translated( double dx, double dy ) const
   {
      GenericMatrix R( *this );
      R.Translate( dx, dy );
      return R;
   }
 
   template <class V>
   GenericMatrix Translated( const V& delta ) const
   {
      return Translated( double( delta[0] ), double( delta[1] ) );
   }
 
   static GenericMatrix RotationX( double sphi, double cphi )
   {
      return GenericMatrix( element( 1 ), element(     0 ), element(     0 ),
                            element( 0 ), element( +cphi ), element( +sphi ),
                            element( 0 ), element( -sphi ), element( +cphi ) );
   }
 
   static GenericMatrix RotationX( double phi )
   {
      double sphi, cphi;
      SinCos( phi, sphi, cphi );
      return RotationX( sphi, cphi );
   }
 
   static GenericMatrix RotationY( double sphi, double cphi )
   {
      return GenericMatrix( element( +cphi ), element( 0 ), element( -sphi ),
                            element(     0 ), element( 1 ), element( 0     ),
                            element( +sphi ), element( 0 ), element( +cphi ) );
   }
 
   static GenericMatrix RotationY( double phi )
   {
      double sphi, cphi;
      SinCos( phi, sphi, cphi );
      return RotationY( sphi, cphi );
   }
 
   static GenericMatrix RotationZ( double sphi, double cphi )
   {
      return GenericMatrix( element( +cphi ), element( +sphi ), element( 0 ),
                            element( -sphi ), element( +cphi ), element( 0 ),
                            element(     0 ), element(     0 ), element( 1 ) );
   }
 
   static GenericMatrix RotationZ( double phi )
   {
      double sphi, cphi;
      SinCos( phi, sphi, cphi );
      return RotationZ( sphi, cphi );
   }
 
   void RotateX( double sphi, double cphi )
   {
      PCL_PRECONDITION( Rows() == 3 && Cols() == 3 )
      EnsureUnique();
      block_iterator __restrict__ A1 = m_data->v[1];
      block_iterator __restrict__ A2 = m_data->v[2];
      double a10 =  cphi*A1[0] + sphi*A2[0];
      double a11 =  cphi*A1[1] + sphi*A2[1];
      double a12 =  cphi*A1[2] + sphi*A2[2];
      double a20 = -sphi*A1[0] + cphi*A2[0];
      double a21 = -sphi*A1[1] + cphi*A2[1];
      double a22 = -sphi*A1[2] + cphi*A2[2];
      A1[0] = element( a10 );
      A1[1] = element( a11 );
      A1[2] = element( a12 );
      A2[0] = element( a20 );
      A2[1] = element( a21 );
      A2[2] = element( a22 );
   }
 
   void RotateX( double phi )
   {
      double sphi, cphi;
      SinCos( phi, sphi, cphi );
      RotateX( sphi, cphi );
   }
 
   GenericMatrix RotatedX( double sphi, double cphi ) const
   {
      GenericMatrix R( *this );
      R.RotateX( sphi, cphi );
      return R;
   }
 
   GenericMatrix RotatedX( double phi ) const
   {
      GenericMatrix R( *this );
      R.RotateX( phi );
      return R;
   }
 
   void RotateY( double sphi, double cphi )
   {
      PCL_PRECONDITION( Rows() == 3 && Cols() == 3 )
      EnsureUnique();
      block_iterator __restrict__ A0 = m_data->v[0];
      block_iterator __restrict__ A2 = m_data->v[2];
      double a00 = cphi*A0[0] - sphi*A2[0];
      double a01 = cphi*A0[1] - sphi*A2[1];
      double a02 = cphi*A0[2] - sphi*A2[2];
      double a20 = sphi*A0[0] + cphi*A2[0];
      double a21 = sphi*A0[1] + cphi*A2[1];
      double a22 = sphi*A0[2] + cphi*A2[2];
      A0[0] = a00;
      A0[1] = a01;
      A0[2] = a02;
      A2[0] = a20;
      A2[1] = a21;
      A2[2] = a22;
   }
 
   void RotateY( double phi )
   {
      double sphi, cphi;
      SinCos( phi, sphi, cphi );
      RotateY( sphi, cphi );
   }
 
   GenericMatrix RotatedY( double sphi, double cphi ) const
   {
      GenericMatrix R( *this );
      R.RotateY( sphi, cphi );
      return R;
   }
 
   GenericMatrix RotatedY( double phi ) const
   {
      GenericMatrix R( *this );
      R.RotateY( phi );
      return R;
   }
 
   void RotateZ( double sphi, double cphi )
   {
      PCL_PRECONDITION( Rows() == 3 && Cols() == 3 )
      EnsureUnique();
      block_iterator __restrict__ A0 = m_data->v[0];
      block_iterator __restrict__ A1 = m_data->v[1];
      double a00 =  cphi*A0[0] + sphi*A1[0];
      double a01 =  cphi*A0[1] + sphi*A1[1];
      double a02 =  cphi*A0[2] + sphi*A1[2];
      double a10 = -sphi*A0[0] + cphi*A1[0];
      double a11 = -sphi*A0[1] + cphi*A1[1];
      double a12 = -sphi*A0[2] + cphi*A1[2];
      A0[0] = a00;
      A0[1] = a01;
      A0[2] = a02;
      A1[0] = a10;
      A1[1] = a11;
      A1[2] = a12;
   }
 
   void RotateZ( double phi )
   {
      double sphi, cphi;
      SinCos( phi, sphi, cphi );
      RotateZ( sphi, cphi );
   }
 
   GenericMatrix RotatedZ( double sphi, double cphi ) const
   {
      GenericMatrix R( *this );
      R.RotateZ( sphi, cphi );
      return R;
   }
 
   GenericMatrix RotatedZ( double phi ) const
   {
      GenericMatrix R( *this );
      R.RotateZ( phi );
      return R;
   }
 
   void Truncate( const element& f0 = element( 0 ), const element& f1 = element( 1 ) )
   {
      EnsureUnique();
      block_iterator __restrict__ i = m_data->Begin();
      block_iterator __restrict__ j = m_data->End();
      PCL_IVDEP
      for ( ; i < j; ++i )
         if ( *i < f0 )
            *i = f0;
         else if ( *i > f1 )
            *i = f1;
   }
 
   GenericMatrix Truncated( const element& f0 = element( 0 ), const element& f1 = element( 1 ) ) const
   {
      GenericMatrix R( *this );
      R.Truncate( f0, f1 );
      return R;
   }
 
   void Rescale( const element& f0 = element( 0 ), const element& f1 = element( 1 ) )
   {
      element v0 = MinElement();
      element v1 = MaxElement();
      if ( v0 != f0 || v1 != f1 )
      {
         EnsureUnique();
         if ( v0 != v1 )
         {
            if ( f0 != f1 )
            {
               block_iterator __restrict__ i = m_data->Begin();
               block_iterator __restrict__ j = m_data->End();
               double d = (double( f1 ) - double( f0 ))/(double( v1 ) - double( v0 ));
               if ( f0 == element( 0 ) )
               {
                  PCL_IVDEP
                  for ( ; i < j; ++i )
                     *i = element( d*(*i - v0) );
               }
               else
               {
                  PCL_IVDEP
                  for ( ; i < j; ++i )
                     *i = element( d*(*i - v0) + f0 );
               }
            }
            else
               pcl::Fill( m_data->Begin(), m_data->End(), f0 );
         }
         else
            pcl::Fill( m_data->Begin(), m_data->End(), pcl::Range( v0, f0, f1 ) );
      }
   }
 
   GenericMatrix Rescaled( const element& f0 = element( 0 ), const element& f1 = element( 1 ) ) const
   {
      GenericMatrix R( *this );
      R.Rescale( f0, f1 );
      return R;
   }
 
   void Sort()
   {
      EnsureUnique();
      pcl::Sort( m_data->Begin(), m_data->End() );
   }
 
   GenericMatrix Sorted() const
   {
      GenericMatrix R( *this );
      R.Sort();
      return R;
   }
 
   void ReverseSort()
   {
      EnsureUnique();
      pcl::Sort( m_data->Begin(), m_data->End(),
                 []( const element& a, const element& b ){ return b < a; } );
   }
 
   GenericMatrix ReverseSorted() const
   {
      GenericMatrix R( *this );
      R.ReverseSort();
      return R;
   }
 
   template <class BP>
   void Sort( BP p )
   {
      EnsureUnique();
      pcl::Sort( m_data->Begin(), m_data->End(), p );
   }
 
   template <class BP>
   GenericMatrix Sorted( BP p ) const
   {
      GenericMatrix R( *this );
      R.Sort( p );
      return R;
   }
 
   const_block_iterator Find( const element& x ) const noexcept
   {
      const_block_iterator p = pcl::LinearSearch( m_data->Begin(), m_data->End(), x );
      return (p != m_data->End()) ? p : nullptr;
   }
 
   const_block_iterator FindFirst( const element& x ) const noexcept
   {
      return Find( x );
   }
 
   const_block_iterator FindLast( const element& x ) const noexcept
   {
      const_block_iterator p = pcl::LinearSearchLast( m_data->Begin(), m_data->End(), x );
      return (p != m_data->End()) ? p : nullptr;
   }
 
   bool Contains( const element& x ) const noexcept
   {
      return pcl::LinearSearch( m_data->Begin(), m_data->End(), x ) != m_data->End();
   }
 
#ifndef __PCL_NO_MATRIX_STATISTICS
 
   element MinElement() const noexcept
   {
      if ( !IsEmpty() )
         return *MinItem( m_data->Begin(), m_data->End() );
      return element( 0 );
   }
 
   element MinElement( Point& p ) const noexcept
   {
      if ( !IsEmpty() )
      {
         const_block_iterator m = MinItem( m_data->Begin(), m_data->End() );
         distance_type d = m - m_data->Begin();
         p.y = int( d/m_data->Cols() );
         p.x = int( d%m_data->Cols() );
         return *m;
      }
      p = 0;
      return element( 0 );
   }
 
   element MaxElement() const noexcept
   {
      if ( !IsEmpty() )
         return *MaxItem( m_data->Begin(), m_data->End() );
      return element( 0 );
   }
 
   element MaxElement( Point& p ) const noexcept
   {
      if ( !IsEmpty() )
      {
         const_block_iterator m = MaxItem( m_data->Begin(), m_data->End() );
         int d = m - m_data->Begin();
         p.y = d/m_data->Cols();
         p.x = d%m_data->Cols();
         return *m;
      }
      p = 0;
      return element( 0 );
   }
 
   double Sum() const noexcept
   {
      return pcl::Sum( m_data->Begin(), m_data->End() );
   }
 
   double StableSum() const noexcept
   {
      return pcl::StableSum( m_data->Begin(), m_data->End() );
   }
 
   double Modulus() const noexcept
   {
      return pcl::Modulus( m_data->Begin(), m_data->End() );
   }
 
   double StableModulus() const noexcept
   {
      return pcl::StableModulus( m_data->Begin(), m_data->End() );
   }
 
   double SumOfSquares() const noexcept
   {
      return pcl::SumOfSquares( m_data->Begin(), m_data->End() );
   }
 
   double StableSumOfSquares() const noexcept
   {
      return pcl::StableSumOfSquares( m_data->Begin(), m_data->End() );
   }
 
   double Mean() const noexcept
   {
      return pcl::Mean( m_data->Begin(), m_data->End() );
   }
 
   double StableMean() const noexcept
   {
      return pcl::StableMean( m_data->Begin(), m_data->End() );
   }
 
   double TrimmedMean( distance_type l = 1, distance_type h = 1 ) const noexcept
   {
      return pcl::TrimmedMean( m_data->Begin(), m_data->End(), l, h );
   }
 
   double TrimmedMeanOfSquares( distance_type l = 1, distance_type h = 1 ) const noexcept
   {
      return pcl::TrimmedMeanOfSquares( m_data->Begin(), m_data->End(), l, h );
   }
 
   double Variance() const noexcept
   {
      return pcl::Variance( m_data->Begin(), m_data->End() );
   }
 
   double StdDev() const noexcept
   {
      return pcl::StdDev( m_data->Begin(), m_data->End() );
   }
 
   double Median() const
   {
      return pcl::Median( m_data->Begin(), m_data->End() );
   }
 
   double AvgDev( double center ) const noexcept
   {
      return pcl::AvgDev( m_data->Begin(), m_data->End(), center );
   }
 
   double StableAvgDev( double center ) const noexcept
   {
      return pcl::StableAvgDev( m_data->Begin(), m_data->End(), center );
   }
 
   double AvgDev() const
   {
      return pcl::AvgDev( m_data->Begin(), m_data->End() );
   }
 
   double StableAvgDev() const
   {
      return pcl::StableAvgDev( m_data->Begin(), m_data->End() );
   }
 
   TwoSidedEstimate TwoSidedAvgDev( double center ) const noexcept
   {
      return pcl::TwoSidedAvgDev( m_data->Begin(), m_data->End(), center );
   }
 
   TwoSidedEstimate TwoSidedAvgDev() const
   {
      return pcl::TwoSidedAvgDev( m_data->Begin(), m_data->End() );
   }
 
   double MAD( double center ) const
   {
      return pcl::MAD( m_data->Begin(), m_data->End(), center );
   }
 
   double MAD() const
   {
      return pcl::MAD( m_data->Begin(), m_data->End() );
   }
 
   TwoSidedEstimate TwoSidedMAD( double center ) const
   {
      return pcl::TwoSidedMAD( m_data->Begin(), m_data->End(), center );
   }
 
   TwoSidedEstimate TwoSidedMAD() const
   {
      return pcl::TwoSidedMAD( m_data->Begin(), m_data->End() );
   }
 
   double BiweightMidvariance( double center, double sigma, int k = 9, bool reducedLength = false ) const noexcept
   {
      return pcl::BiweightMidvariance( m_data->Begin(), m_data->End(), center, sigma, k, reducedLength );
   }
 
   double BiweightMidvariance( int k = 9, bool reducedLength = false ) const
   {
      double center = Median();
      return BiweightMidvariance( center, MAD( center ), k, reducedLength );
   }
 
   TwoSidedEstimate TwoSidedBiweightMidvariance( double center, const TwoSidedEstimate& sigma,
                                                 int k = 9, bool reducedLength = false ) const noexcept
   {
      return pcl::TwoSidedBiweightMidvariance( m_data->Begin(), m_data->End(), center, sigma, k, reducedLength );
   }
 
   TwoSidedEstimate TwoSidedBiweightMidvariance( int k = 9, bool reducedLength = false ) const
   {
      double center = Median();
      return TwoSidedBiweightMidvariance( center, TwoSidedMAD( center ), k, reducedLength );
   }
 
   double BendMidvariance( double center, double beta = 0.2 ) const
   {
      return pcl::BendMidvariance( m_data->Begin(), m_data->End(), center, beta );
   }
 
   double BendMidvariance( double beta = 0.2 ) const
   {
      return pcl::BendMidvariance( m_data->Begin(), m_data->End(), Median(), beta );
   }
 
   double Sn() const
   {
      GenericMatrix A( *this );
      return pcl::Sn( A.Begin(), A.End() );
   }
 
   double Qn() const
   {
      GenericMatrix A( *this );
      return pcl::Qn( A.Begin(), A.End() );
   }
 
#endif   // !__PCL_NO_MATRIX_STATISTICS
 
   uint64 Hash64( uint64 seed = 0 ) const noexcept
   {
      return pcl::Hash64( m_data->Begin(), m_data->Size(), seed );
   }
 
   uint32 Hash32( uint32 seed = 0 ) const noexcept
   {
      return pcl::Hash32( m_data->Begin(), m_data->Size(), seed );
   }
 
   uint64 Hash( uint64 seed = 0 ) const noexcept
   {
      return Hash64( seed );
   }
 
   template <class S, typename SP>
   S& ToSeparated( S& s, SP separator ) const
   {
      const_block_iterator i = m_data->Begin(), j = m_data->End();
      if ( i < j )
      {
         s.Append( S( *i ) );
         if ( ++i < j )
            do
            {
               s.Append( separator );
               s.Append( S( *i ) );
            }
            while ( ++i < j );
      }
      return s;
   }
 
   template <class S, typename SP, class AF>
   S& ToSeparated( S& s, SP separator, AF append ) const
   {
      const_block_iterator i = m_data->Begin(), j = m_data->End();
      if ( i < j )
      {
         append( s, S( *i ) );
         if ( ++i < j )
         {
            S p( separator );
            do
            {
               append( s, p );
               append( s, S( *i ) );
            }
            while ( ++i < j );
         }
      }
      return s;
   }
 
   template <class S>
   S& ToCommaSeparated( S& s ) const
   {
      return ToSeparated( s, ',' );
   }
 
   template <class S>
   S& ToSpaceSeparated( S& s ) const
   {
      return ToSeparated( s, ' ' );
   }
 
   template <class S>
   S& ToTabSeparated( S& s ) const
   {
      return ToSeparated( s, '\t' );
   }
 
#ifndef __PCL_NO_MATRIX_PHASE_MATRICES
 
   static GenericMatrix PhaseCorrelationMatrix( const GenericMatrix& A, const GenericMatrix& B )
   {
      if ( B.Rows() != A.Rows() || B.Cols() != A.Cols() )
         throw Error( "Invalid matrix dimensions in PhaseCorrelationMatrix()" );
      GenericMatrix R( A.Rows(), A.Cols() );
      PhaseCorrelationMatrix( R.Begin(), R.End(), A.Begin(), B.Begin() );
      return R;
   }
 
   static GenericMatrix CrossPowerSpectrumMatrix( const GenericMatrix& A, const GenericMatrix& B )
   {
      if ( B.Rows() != A.Rows() || B.Cols() != A.Cols() )
         throw Error( "Invalid matrix dimensions in CrossPowerSpectrumMatrix()" );
      GenericMatrix R( A.Rows(), A.Cols() );
      CrossPowerSpectrumMatrix( R.Begin(), R.End(), A.Begin(), B.Begin() );
      return R;
   }
 
#endif   // !__PCL_NO_MATRIX_PHASE_MATRICES
 
#ifndef __PCL_NO_MATRIX_IMAGE_RENDERING
 
   template <class P>
   void ToImage( GenericImage<P>& image ) const
   {
      image.AllocateData( Cols(), Rows() );
      typename P::sample*  __restrict__ v = *image;
      const_block_iterator __restrict__ i = m_data->Begin();
      const_block_iterator __restrict__ j = m_data->End();
      PCL_IVDEP
      for ( ; i < j; ++i, ++v )
         *v = P::ToSample( *i );
   }
 
   void ToImage( ImageVariant& image ) const
   {
      if ( !image )
         image.CreateFloatImage();
 
      if ( image.IsFloatSample() )
         switch ( image.BitsPerSample() )
         {
         case 32: ToImage( static_cast<Image&>( *image ) ); break;
         case 64: ToImage( static_cast<DImage&>( *image ) ); break;
         }
      else if ( image.IsComplexSample() )
         switch ( image.BitsPerSample() )
         {
         case 32: ToImage( static_cast<ComplexImage&>( *image ) ); break;
         case 64: ToImage( static_cast<DComplexImage&>( *image ) ); break;
         }
      else
         switch ( image.BitsPerSample() )
         {
         case  8: ToImage( static_cast<UInt8Image&>( *image ) ); break;
         case 16: ToImage( static_cast<UInt16Image&>( *image ) ); break;
         case 32: ToImage( static_cast<UInt32Image&>( *image ) ); break;
         }
   }
 
#endif   // !__PCL_NO_MATRIX_IMAGE_RENDERING
 
#ifndef __PCL_NO_MATRIX_IMAGE_CONVERSION
 
   template <class P>
   static GenericMatrix FromImage( const GenericImage<P>& image, const Rect& rect = Rect( 0 ), int channel = -1 )
   {
      Rect r = rect;
      if ( !image.ParseSelection( r, channel ) )
         return GenericMatrix();
 
      if ( r == image.Bounds() )
      {
         GenericMatrix M( image.Height(), image.Width() );
         const typename P::sample* __restrict__ v = image[channel];
               block_iterator      __restrict__ i = M.m_data->Begin();
         const_block_iterator      __restrict__ j = M.m_data->End();
         PCL_IVDEP
         for ( ; i < j; ++i, ++v )
            P::FromSample( *i, *v );
         return M;
      }
      else
      {
         int w = r.Width();
         int h = r.Height();
         GenericMatrix M( h, w );
         block_iterator __restrict__ m = M.m_data->Begin();
         PCL_IVDEP
         for ( int i = r.y0; i < r.y1; ++i )
         {
            const typename P::sample* __restrict__ v = image.PixelAddress( r.x0, i, channel );
            PCL_IVDEP
            for ( int j = 0; j < w; ++j )
               P::FromSample( *m++, *v++ );
         }
         return M;
      }
   }
 
   static GenericMatrix FromImage( const ImageVariant& image, const Rect& rect = Rect( 0 ), int channel = -1 )
   {
      if ( image )
         if ( image.IsFloatSample() )
            switch ( image.BitsPerSample() )
            {
            case 32: return FromImage( static_cast<const Image&>( *image ), rect, channel );
            case 64: return FromImage( static_cast<const DImage&>( *image ), rect, channel );
            }
         else if ( image.IsComplexSample() )
            switch ( image.BitsPerSample() )
            {
            case 32: return FromImage( static_cast<const ComplexImage&>( *image ), rect, channel );
            case 64: return FromImage( static_cast<const DComplexImage&>( *image ), rect, channel );
            }
         else
            switch ( image.BitsPerSample() )
            {
            case  8: return FromImage( static_cast<const UInt8Image&>( *image ), rect, channel );
            case 16: return FromImage( static_cast<const UInt16Image&>( *image ), rect, channel );
            case 32: return FromImage( static_cast<const UInt32Image&>( *image ), rect, channel );
            }
 
      return GenericMatrix();
   }
 
#endif   // !__PCL_NO_MATRIX_IMAGE_CONVERSION
 
private:
 
   struct Data : public ReferenceCounter
   {
      int             n = 0;       
      int             m = 0;       
      block_iterator* v = nullptr; 
 
      Data() = default;
 
      Data( int rows, int cols )
      {
         if ( rows > 0 && cols > 0 )
            Allocate( rows, cols );
      }
 
      ~Data()
      {
         Deallocate();
      }
 
      int Rows() const noexcept
      {
         return n;
      }
 
      int Cols() const noexcept
      {
         return m;
      }
 
      size_type NumberOfElements() const noexcept
      {
         return size_type( n )*size_type( m );
      }
 
      size_type Size() const noexcept
      {
         return NumberOfElements()*sizeof( element );
      }
 
      block_iterator Begin() const
      {
         block_iterator p = (v != nullptr) ? *v : nullptr;
         if ( likely( std::is_scalar<element>::value ) )
            return reinterpret_cast<block_iterator>( PCL_ASSUME_ALIGNED_32( p ) );
         return p;
      }
 
      block_iterator End() const noexcept
      {
         return (v != nullptr) ? *v + NumberOfElements() : nullptr;
      }
 
      element& Element( int i, int j ) const noexcept
      {
         return v[i][j];
      }
 
      void Allocate( int rows, int cols )
      {
         n = rows;
         m = cols;
         v = new block_iterator[ n ];
         if ( likely( std::is_scalar<element>::value ) )
         {
            *v = reinterpret_cast<block_iterator>( PCL_ALIGNED_MALLOC( Size(), 32 ) );
            if ( unlikely( *v == nullptr ) )
            {
               delete [] v;
               v = nullptr;
               n = m = 0;
               throw std::bad_alloc();
            }
         }
         else
            *v = new element[ NumberOfElements() ];
         for ( int i = 1; i < n; ++i )
            v[i] = v[i-1] + m;
      }
 
      void Deallocate()
      {
         PCL_PRECONDITION( refCount == 0 )
         if ( v != nullptr )
         {
            if ( likely( std::is_scalar<element>::value ) )
               PCL_ALIGNED_FREE( *v );
            else
               delete [] *v;
            delete [] v;
            v = nullptr;
            n = m = 0;
         }
      }
   };
 
   Data* m_data = nullptr;
 
   void DetachFromData()
   {
      if ( !m_data->Detach() )
         delete m_data;
   }
 
   static bool Invert( block_iterator* Ai, const_block_iterator const* A, int n ) noexcept
   {
      switch ( n )
      {
      case 1:
         if ( 1 + **A == 1 )
            return false;
         **Ai = 1/(**A);
         break;
      case 2:
         {
            const_block_iterator __restrict__ A0 = A[0];
            const_block_iterator __restrict__ A1 = A[1];
            element d = A0[0]*A1[1] - A0[1]*A1[0];
            if ( 1 + d == 1 )
               return false;
            Ai[0][0] =  A1[1]/d;
            Ai[0][1] = -A0[1]/d;
            Ai[1][0] = -A1[0]/d;
            Ai[1][1] =  A0[0]/d;
         }
         break;
      case 3:
         {
            const_block_iterator __restrict__ A0 = A[0];
            const_block_iterator __restrict__ A1 = A[1];
            const_block_iterator __restrict__ A2 = A[2];
            element d1 = A1[1]*A2[2] - A1[2]*A2[1];
            element d2 = A1[2]*A2[0] - A1[0]*A2[2];
            element d3 = A1[0]*A2[1] - A1[1]*A2[0];
            element d  = A0[0]*d1 + A0[1]*d2 + A0[2]*d3;
            if ( 1 + d == 1 )
               return false;
            Ai[0][0] = d1/d;
            Ai[0][1] = (A2[1]*A0[2] - A2[2]*A0[1])/d;
            Ai[0][2] = (A0[1]*A1[2] - A0[2]*A1[1])/d;
            Ai[1][0] = d2/d;
            Ai[1][1] = (A2[2]*A0[0] - A2[0]*A0[2])/d;
            Ai[1][2] = (A0[2]*A1[0] - A0[0]*A1[2])/d;
            Ai[2][0] = d3/d;
            Ai[2][1] = (A2[0]*A0[1] - A2[1]*A0[0])/d;
            Ai[2][2] = (A0[0]*A1[1] - A0[1]*A1[0])/d;
         }
         break;
      default: // ?!
         return false;
      }
      return true;
   }
};
 
// ### N.B.: Visual C++ is unable to compile the next two member functions if
//           the following external function declarations are placed within the
//           member function bodies. This forces us to implement them after the
//           GenericMatrix<> class declaration.
 
void PCL_FUNC InPlaceGaussJordan( GenericMatrix<double>&, GenericMatrix<double>& );
void PCL_FUNC InPlaceGaussJordan( GenericMatrix<float>&, GenericMatrix<float>& );
 
template <typename T> inline
GenericMatrix<T> GenericMatrix<T>::Inverse() const
{
   if ( Rows() != Cols() || Rows() == 0 )
      throw Error( "Invalid matrix inversion: Non-square or empty matrix." );
 
   /*
    * - Use direct formulae to invert 1x1, 2x2 and 3x3 matrices.
    * - Use Gauss-Jordan elimination to invert 4x4 and larger matrices.
    */
   if ( Rows() < 4 )
   {
      GenericMatrix Ai( Rows(), Rows() );
      if ( !Invert( Ai.m_data->v, m_data->v, Rows() ) )
         throw Error( "Invalid matrix inversion: Singular matrix." );
      return Ai;
   }
   else
   {
      GenericMatrix Ai( *this );
      GenericMatrix B = UnitMatrix( Rows() );
      InPlaceGaussJordan( Ai, B );
      return Ai;
   }
}
 
template <typename T> inline
void GenericMatrix<T>::Invert()
{
   if ( Rows() <= 3 )
      Transfer( Inverse() );
   else
   {
      if ( Rows() != Cols() || Rows() == 0 )
         throw Error( "Invalid matrix inversion: Non-square or empty matrix." );
      EnsureUnique();
      GenericMatrix B = UnitMatrix( Rows() );
      InPlaceGaussJordan( *this, B );
   }
}
 
// ----------------------------------------------------------------------------
 
template <typename T> inline
GenericMatrix<T> operator +( const GenericMatrix<T>& A, const GenericMatrix<T>& B )
{
   if ( B.Rows() != A.Rows() || B.Cols() != A.Cols() )
      throw Error( "Invalid matrix addition." );
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ t = B.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k, ++t )
      *i = *k + *t;
   return R;
}
 
template <typename T> inline
GenericMatrix<T> operator +( const GenericMatrix<T>& A, const T& x )
{
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k )
      *i = *k + x;
   return R;
}
 
template <typename T> inline
GenericMatrix<T> operator +( const T& x, const GenericMatrix<T>& A )
{
   return A + x;
}
 
// ----------------------------------------------------------------------------
 
template <typename T> inline
GenericMatrix<T> operator -( const GenericMatrix<T>& A, const GenericMatrix<T>& B )
{
   if ( B.Rows() != A.Rows() || B.Cols() != A.Cols() )
      throw Error( "Invalid matrix subtraction." );
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ t = B.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k, ++t )
      *i = *k - *t;
   return R;
}
 
template <typename T> inline
GenericMatrix<T> operator -( const GenericMatrix<T>& A, const T& x )
{
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k )
      *i = *k - x;
   return R;
}
 
template <typename T> inline
GenericMatrix<T> operator -( const T& x, const GenericMatrix<T>& A )
{
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k )
      *i = x - *k;
   return R;
}
 
// ----------------------------------------------------------------------------
 
template <typename T> inline
GenericMatrix<T> operator *( const GenericMatrix<T>& A, const GenericMatrix<T>& B )
{
   int n = A.Rows();
   int m = B.Cols();
   int p = A.Cols();
   if ( B.Rows() != p )
      throw Error( "Invalid matrix multiplication." );
   GenericMatrix<T> R( n, m );
   for ( int i = 0; i < n; ++i )
      for ( int j = 0; j < m; ++j )
      {
         T rij = 0;
         for ( int k = 0; k < p; ++k )
            rij += A[i][k] * B[k][j];
         R[i][j] = rij;
      }
   return R;
}
 
template <typename T> inline
GenericVector<T> operator *( const GenericMatrix<T>& A, const GenericVector<T>& x )
{
   int n = A.Cols();
   int m = A.Rows();
   if ( x.Length() != n )
      throw Error( "Invalid matrix-vector multiplication." );
   GenericVector<T> r( m );
   for ( int i = 0; i < m; ++i )
   {
      T ri = 0;
      for ( int j = 0; j < n; ++j )
         ri += A[i][j] * x[j];
      r[i] = ri;
   }
   return r;
}
 
template <typename T> inline
GenericMatrix<T> operator *( const GenericMatrix<T>& A, const T& x )
{
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k )
      *i = *k * x;
   return R;
}
 
template <typename T> inline
GenericMatrix<T> operator *( const T& x, const GenericMatrix<T>& A )
{
   return A * x;
}
 
// ----------------------------------------------------------------------------
 
template <typename T> inline
GenericMatrix<T> operator /( const GenericMatrix<T>& A, const T& x )
{
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k )
      *i = *k / x;
   return R;
}
 
template <typename T> inline
GenericMatrix<T> operator /( const T& x, const GenericMatrix<T>& A )
{
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k )
      *i = x / *k;
   return R;
}
 
// ----------------------------------------------------------------------------
 
template <typename T> inline
GenericMatrix<T> operator ^( const GenericMatrix<T>& A, const T& x )
{
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k )
      *i = pcl::Pow( *k, x );
   return R;
}
 
template <typename T> inline
GenericMatrix<T> operator ^( const T& x, const GenericMatrix<T>& A )
{
   GenericMatrix<T> R( A.Rows(), A.Cols() );
   typename       GenericMatrix<T>::block_iterator __restrict__ i = R.Begin();
   typename GenericMatrix<T>::const_block_iterator __restrict__ j = R.End();
   typename GenericMatrix<T>::const_block_iterator __restrict__ k = A.Begin();
   PCL_IVDEP
   for ( ; i < j; ++i, ++k )
      *i = pcl::Pow( x, *k );
   return R;
}
 
// ----------------------------------------------------------------------------
 
#ifndef __PCL_NO_MATRIX_INSTANTIATE
 
using I8Matrix = GenericMatrix<int8>;
 
using CharMatrix = I8Matrix;
 
using UI8Matrix = GenericMatrix<uint8>;
 
using ByteMatrix = UI8Matrix;
 
using I16Matrix = GenericMatrix<int16>;
 
using UI16Matrix = GenericMatrix<uint16>;
 
using I32Matrix = GenericMatrix<int32>;
 
using IMatrix = I32Matrix;
 
using UI32Matrix = GenericMatrix<uint32>;
 
using UIMatrix = UI32Matrix;
 
using I64Matrix = GenericMatrix<int64>;
 
using UI64Matrix = GenericMatrix<uint64>;
 
using F32Matrix = GenericMatrix<float>;
 
using FMatrix = F32Matrix;
 
using F64Matrix = GenericMatrix<double>;
 
using DMatrix = F64Matrix;
 
using Matrix = DMatrix;
 
using C32Matrix = GenericMatrix<Complex32>;
 
using C64Matrix = GenericMatrix<Complex64>;
 
#ifndef _MSC_VER
 
using F80Matrix = GenericMatrix<long double>;
 
using LDMatrix = F80Matrix;
 
#endif   // !_MSC_VER
 
#endif   // !__PCL_NO_MATRIX_INSTANTIATE
 
// ----------------------------------------------------------------------------
 
} // pcl
 
#endif   // __PCL_Matrix_h
 
// ----------------------------------------------------------------------------
// EOF pcl/Matrix.h - Released 2024-06-18T15:48:54Z