source/coredecomp.cc

   1 /* This file is part of Shapes.
   2  *
   3  * Shapes is free software: you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5  * the Free Software Foundation, either version 3 of the License, or
   6  * any later version.
   7  *
   8  * Shapes is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with Shapes.  If not, see <http://www.gnu.org/licenses/>.
  15  *
  16  * Copyright 2008 Henrik Tidefelt
  17  */
  18
  19 #include <cmath>
  20
  21 #include "shapescore.h"
  22 #include "globals.h"
  23 #include "shapesexceptions.h"
  24 #include "consts.h"
  25 #include "astfun.h"
  26 #include "continuations.h"
  27
  28 #include <gsl/gsl_matrix.h>
  29 #include <gsl/gsl_linalg.h>
  30 #include <gsl/gsl_blas.h>
  31 #include <gsl/gsl_eigen.h>
  32 #include <gsl/gsl_complex_math.h>
  33 #include <iostream>
  34 #include <sstream>
  35
  36 using namespace Shapes;
  37
  38
  39 namespace Shapes
  40 {
  41         namespace Helpers
  42         {
  43                 void Schur_decomposition_helper_2D( Kernel::EvalState * evalState, const RefCountPtr< const Lang::Transform2D > & tf, int rank, bool canonical, const Ast::SourceLocation & callLoc );
  44                 void Schur_decomposition_helper_3D( Kernel::EvalState * evalState, const RefCountPtr< const Lang::Transform3D > & tf, int rank, bool canonical, const Ast::SourceLocation & callLoc );
  45                 Kernel::StructureFactory Schur_decomposition_resultFactory( "Q", "U" );
  46
  47                 void minRotationMatrix3D( const Concrete::UnitFloatTriple & from, const Concrete::UnitFloatTriple & to, gsl_matrix * dst_3_3 );
  48         }
  49 }
  50
  51 namespace Shapes
  52 {
  53         namespace Lang
  54         {
  55                 class Core_Schur_decomposition : public Lang::CoreFunction
  56                 {
  57                 public:
  58                         Core_Schur_decomposition( const char * title )
  59                                 : CoreFunction( title, new Kernel::EvaluatedFormals( Ast::FileID::build_internal( title ), true ) )
  60                         {
  61                                 formals_->appendEvaluatedCoreFormal( "tf", Kernel::THE_SLOT_VARIABLE );
  62                                 formals_->appendEvaluatedCoreFormal( "rank", Kernel::THE_VOID_VARIABLE );
  63                                 formals_->appendEvaluatedCoreFormal( "canonical", Kernel::THE_FALSE_VARIABLE );
  64                         }
  65                         virtual void
  66                         call( Kernel::EvalState * evalState, Kernel::Arguments & args, const Ast::SourceLocation & callLoc ) const
  67                         {
  68                                 args.applyDefaults( );
  69
  70                                 size_t argsi = 1;
  71
  72                                 int rank = -1;
  73                                 typedef const Lang::Integer RankType;
  74                                 RefCountPtr< RankType > rankPtr = Helpers::down_cast_CoreArgument< RankType >( title_, args, argsi, callLoc, true );
  75                                 if( rankPtr != NullPtr< RankType >( ) )
  76                                         {
  77                                                 rank = rankPtr->val_;
  78                                                 if( rank < 0 )
  79                                                         {
  80                                                                 throw Exceptions::CoreOutOfRange( title_, args, argsi, "Rank is negative." );
  81                                                         }
  82                                         }
  83
  84                                 ++argsi;
  85                                 typedef const Lang::Boolean FlagType;
  86                                 bool canonical = Helpers::down_cast_CoreArgument< FlagType >( title_, args, argsi, callLoc )->val_;
  87
  88                                 argsi = 0;
  89                                 try
  90                                         {
  91                                                 typedef const Lang::Transform2D ArgType;
  92                                                 RefCountPtr< ArgType > tf = Helpers::try_cast_CoreArgument< ArgType >( args.getValue( argsi ) );
  93                                                 if( rank > 2 )
  94                                                         {
  95                                                                 throw Exceptions::CoreOutOfRange( title_, args, argsi, "Rank exceeds dimension." );
  96                                                         }
  97                                                 Helpers::Schur_decomposition_helper_2D( evalState, tf, rank, canonical, callLoc );
  98                                                 return;
  99                                         }
 100                                 catch( const NonLocalExit::NotThisType & ball )
 101                                         {
 102                                                 /* Wrong type; never mind!.. but see below!
 103                                                  */
 104                                         }
 105
 106                                 try
 107                                         {
 108                                                 typedef const Lang::Transform3D ArgType;
 109                                                 RefCountPtr< ArgType > tf = Helpers::try_cast_CoreArgument< ArgType >( args.getValue( argsi ) );
 110                                                 if( rank > 3 )
 111                                                         {
 112                                                                 throw Exceptions::CoreOutOfRange( title_, args, argsi, "Rank exceeds dimension." );
 113                                                         }
 114                                                 Helpers::Schur_decomposition_helper_3D( evalState, tf, rank, canonical, callLoc );
 115                                                 return;
 116                                         }
 117                                 catch( const NonLocalExit::NotThisType & ball )
 118                                         {
 119                                                 /* Wrong type; never mind!.. but see below!
 120                                                  */
 121                                         }
 122
 123                                 throw Exceptions::CoreTypeMismatch( callLoc, title_, args, 0, Helpers::typeSetString( Lang::Transform2D::staticTypeName( ), Lang::Transform3D::staticTypeName( ) ) );
 124                         }
 125                 };
 126         }
 127 }
 128
 129 namespace Shapes
 130 {
 131         namespace Helpers
 132         {
 133                 void
 134                 Schur_decomposition_helper_2D( Kernel::EvalState * evalState, const RefCountPtr< const Lang::Transform2D > & tf, int rank, bool canonical, const Ast::SourceLocation & callLoc )
 135                 {
 136                         throw Exceptions::NotImplemented( "Schur_decomposition_helper_2D" );
 137                 }
 138
 139                 void
 140                 Schur_decomposition_helper_3D( Kernel::EvalState * evalState, const RefCountPtr< const Lang::Transform3D > & tf, int rank, bool canonical, const Ast::SourceLocation & callLoc )
 141                 {
 142                         const size_t N = 3;
 143                         static gsl_eigen_nonsymm_workspace * ws = gsl_eigen_nonsymm_alloc( N );
 144                         gsl_eigen_nonsymm_params( 1, 0, ws );
 145                         static gsl_matrix * L = gsl_matrix_alloc( N, N );
 146                         static gsl_vector * p = gsl_vector_alloc( N );
 147                         static gsl_matrix * Q = gsl_matrix_alloc( N, N );
 148                         static gsl_vector * Qp = gsl_vector_alloc( N );
 149                         static gsl_vector_complex * eval = gsl_vector_complex_alloc( N );
 150                         tf->write_gsl_matrix( L );
 151                         if( gsl_eigen_nonsymm_Z( L, eval, Q, ws ) != 0 )
 152                                 {
 153                                         throw Exceptions::ExternalError( "gsl_eigen_nonsymm_Z failed.  (This is one of those \"rare occations\"...)" );
 154                                 }
 155                         /* Now, we ensure that the upper block triangular matrix is in what we consider canonical form,
 156                          * namely that any 2x2 block on the diagonal comes before the 1x1 blocks.
 157                          */
 158                         if( fabs( gsl_matrix_get( L, 1, 0 ) ) < fabs( gsl_matrix_get( L, 2, 1 ) ) )
 159                                 {
 160                                         gsl_matrix_swap_columns( Q, 0, 2 );
 161                                         gsl_matrix_swap_rows( L, 0, 2 );
 162                                         gsl_matrix_swap_columns( L, 0, 2 );
 163                                         /* Perhaps we should also switch the order of the eigenvalues in eval here, but they seem
 164                                          * unrelated anyway.
 165                                          */
 166                                 }
 167                         if( canonical )
 168                                 {
 169                                         /* If there is a 2x2 block, we use the one degree of freedom this gives for Q, to make the angle of
 170                                          * of rotation in Q as small as possible.
 171                                          * The most reliable test of a 2x2 block that I can think of now is to check for complex eigenvalues.
 172                                          * At most one of the eigenvalues can be real, so testing the maximum angle of any two will reveal the answer.
 173                                          * (If I could rely on the order of the elements in eval, I would just have to test one argument.)
 174                                          */
 175                                         if( std::max( fabs( gsl_complex_arg( gsl_vector_complex_get( eval, 0 ) ) ), fabs( gsl_complex_arg( gsl_vector_complex_get( eval, 1 ) ) ) ) > 1e-4 )
 176                                                 {
 177                                                         /* The direction of rotation given by the third column of Q.  It is already normalized.
 178                                                          * The job that Q must do is to map the (0,0,1) direction to the direction of rotation.
 179                                                          */
 180                                                         Helpers::minRotationMatrix3D( Concrete::UnitFloatTriple( bool( ), bool( ), bool( ) ), /* This is the z direction. */
 181                                                                                                                                                                                 Concrete::UnitFloatTriple( gsl_matrix_get( Q, 0, 2 ), gsl_matrix_get( Q, 1, 2 ), gsl_matrix_get( Q, 2, 2 ), bool( ) ),
 182                                                                                                                                                                                 Q );
 183                                                 }
 184                                         else
 185                                                 {
 186                                                         /* The only thing we can do is to ensure that the Q is special.
 187                                                          */
 188                                                         Concrete::UnitFloatTriple q3 =
 189                                                                 Concrete::crossDirection( Concrete::UnitFloatTriple( gsl_matrix_get( Q, 0, 0 ), gsl_matrix_get( Q, 1, 0 ), gsl_matrix_get( Q, 2, 0 ), bool( ) ),
 190                                                                                                                                                                         Concrete::UnitFloatTriple( gsl_matrix_get( Q, 0, 1 ), gsl_matrix_get( Q, 1, 1 ), gsl_matrix_get( Q, 2, 1 ), bool( ) ) );
 191                                                         gsl_matrix_set( Q, 0, 2, q3.x_ );
 192                                                         gsl_matrix_set( Q, 1, 2, q3.y_ );
 193                                                         gsl_matrix_set( Q, 2, 2, q3.z_ );
 194                                                 }
 195                                         /* Compute new L.
 196                                          */
 197                                         static gsl_matrix * tmp1 = gsl_matrix_alloc( N, N );
 198                                         static gsl_matrix * tmp2 = gsl_matrix_alloc( N, N );
 199                                         tf->write_gsl_matrix( tmp1 );
 200                                         gsl_blas_dgemm( CblasTrans, CblasNoTrans, 1, Q, tmp1, 0, tmp2 );
 201                                         gsl_blas_dgemm( CblasNoTrans, CblasNoTrans, 1, tmp2, Q, 0, L );
 202                                 }
 203                         static gsl_vector * QTp = gsl_vector_alloc( N );
 204                         tf->write_gsl_vector( p );
 205                         gsl_blas_dgemv( CblasTrans, 1, Q, p, 0, QTp );
 206                         if( rank == 0 )
 207                                 {
 208                                         gsl_vector_set_zero( Qp );
 209                                 }
 210                         else
 211                                 {
 212                                         /* Compute the translational part of the change of coordinates.
 213                                          * The goal is to make the transform as linear as possible in the new coordinates.
 214                                          */
 215                                         static gsl_matrix * IsubL = gsl_matrix_alloc( N, N );
 216                                         gsl_matrix_set_identity( IsubL );
 217                                         gsl_matrix_sub( IsubL, L );
 218                                         static gsl_matrix * A = gsl_matrix_alloc( N, N );
 219                                         gsl_blas_dgemm( CblasNoTrans, CblasTrans, 1, IsubL, Q, 0, A );
 220                                         static gsl_vector * SVD_work = gsl_vector_alloc( N );
 221                                         static gsl_vector * sigma = gsl_vector_alloc( N );
 222                                         static gsl_matrix * V = gsl_matrix_alloc( N, N );
 223                                         gsl_linalg_SV_decomp( A, V, sigma, SVD_work );
 224                                         if( rank > 0 )
 225                                                 {
 226                                                         for( size_t i = rank; i < N; ++i )
 227                                                                 {
 228                                                                         gsl_vector_set( sigma, i, 0 );
 229                                                                 }
 230                                                 }
 231                                         else
 232                                                 {
 233                                                         /* A negative value of rank means that the rank shall be determined automatically.
 234                                                          */
 235                                                         const double lim = 1e-2 * gsl_vector_get( sigma, 0 );
 236                                                         for( size_t i = 0; i < N; ++i )
 237                                                                 {
 238                                                                         if( fabs( gsl_vector_get( sigma, i ) ) < lim )
 239                                                                                 {
 240                                                                                         gsl_vector_set( sigma, i, 0 );
 241                                                                                 }
 242                                                                 }
 243                                                 }
 244                                         gsl_linalg_SV_solve( A, V, sigma, QTp, Qp );
 245                                 }
 246                         /* Given Qp, we now compute the offset of the upper block triangular transform. */
 247                         static gsl_vector * Up = gsl_vector_alloc( N );
 248                         gsl_blas_dcopy( QTp, Up );
 249                         static gsl_vector * QTQq = gsl_vector_alloc( N );
 250                         gsl_blas_dgemv( CblasTrans, 1, Q, Qp, 0, QTQq );
 251                         gsl_vector_sub( Up, QTQq );
 252                         gsl_blas_dgemv( CblasNoTrans, 1, L, QTQq, 1, Up );
 253
 254                         Schur_decomposition_resultFactory.set( "Q", Helpers::newValHandle( new Lang::Transform3D( Q, Qp ) ) );
 255                         Schur_decomposition_resultFactory.set( "U", Helpers::newValHandle( new Lang::Transform3D( L, Up ) ) );
 256
 257                         Kernel::ContRef cont = evalState->cont_;
 258                         cont->takeValue( Schur_decomposition_resultFactory.build( ),
 259                                                                                          evalState );
 260                 }
 261         }
 262 }
 263
 264 void
 265 Helpers::minRotationMatrix3D( const Concrete::UnitFloatTriple & from, const Concrete::UnitFloatTriple & to, gsl_matrix * dst_3_3 )
 266 {
 267         /* The rotation maps:
 268          *   from -> to
 269          *   r    -> r
 270          *   a3   -> b3
 271          * as defined below.
 272          */
 273         try
 274                 {
 275                         Concrete::UnitFloatTriple r = Concrete::crossDirection( from, to );
 276                         Concrete::UnitFloatTriple a3 = Concrete::crossDirection( from, r );
 277                         Concrete::UnitFloatTriple b3 = Concrete::crossDirection( to, r );
 278                         static gsl_matrix * A = gsl_matrix_alloc( 3, 3 );
 279                         static gsl_matrix * B = gsl_matrix_alloc( 3, 3 );
 280                         gsl_matrix_set( A, 0, 0, from.x_ );
 281                         gsl_matrix_set( A, 1, 0, from.y_ );
 282                         gsl_matrix_set( A, 2, 0, from.z_ );
 283                         gsl_matrix_set( A, 0, 1, r.x_ );
 284                         gsl_matrix_set( A, 1, 1, r.y_ );
 285                         gsl_matrix_set( A, 2, 1, r.z_ );
 286                         gsl_matrix_set( A, 0, 2, a3.x_ );
 287                         gsl_matrix_set( A, 1, 2, a3.y_ );
 288                         gsl_matrix_set( A, 2, 2, a3.z_ );
 289                         gsl_matrix_set( B, 0, 0, to.x_ );
 290                         gsl_matrix_set( B, 1, 0, to.y_ );
 291                         gsl_matrix_set( B, 2, 0, to.z_ );
 292                         gsl_matrix_set( B, 0, 1, r.x_ );
 293                         gsl_matrix_set( B, 1, 1, r.y_ );
 294                         gsl_matrix_set( B, 2, 1, r.z_ );
 295                         gsl_matrix_set( B, 0, 2, b3.x_ );
 296                         gsl_matrix_set( B, 1, 2, b3.y_ );
 297                         gsl_matrix_set( B, 2, 2, b3.z_ );
 298                         gsl_blas_dgemm( CblasNoTrans, CblasTrans, 1, B, A, 0, dst_3_3 );
 299                 }
 300         catch( const NonLocalExit::CrossDirectionOfParallel & ball )
 301                 {
 302                         gsl_matrix_set_identity( dst_3_3 );
 303                 }
 304 }
 305
 306 namespace Shapes
 307 {
 308         namespace Lang
 309         {
 310                 class Core_rotationMapping3D : public Lang::CoreFunction
 311                 {
 312                 public:
 313                         Core_rotationMapping3D( const char * title )
 314                                 : CoreFunction( title, new Kernel::EvaluatedFormals( Ast::FileID::build_internal( title ), true ) )
 315                         {
 316                                 formals_->appendEvaluatedCoreFormal( "from", Kernel::THE_SLOT_VARIABLE );
 317                                 formals_->appendEvaluatedCoreFormal( "to", Kernel::THE_SLOT_VARIABLE );
 318                         }
 319                         virtual void
 320                         call( Kernel::EvalState * evalState, Kernel::Arguments & args, const Ast::SourceLocation & callLoc ) const
 321                         {
 322                                 args.applyDefaults( );
 323
 324                                 size_t i = 0;
 325
 326                                 typedef const Lang::FloatTriple ArgType;
 327                                 RefCountPtr< ArgType > from = Helpers::down_cast_CoreArgument< ArgType >( title_, args, i, callLoc );
 328                                 if( from->x_ == 0 && from->y_ == 0 && from->z_ == 0 )
 329                                         {
 330                                                 throw Exceptions::CoreOutOfRange( title_, args, i, "The <from> direction is degenerate, that is (0,0,0)." );
 331                                         }
 332
 333                                 ++i;
 334                                 RefCountPtr< ArgType > to = Helpers::down_cast_CoreArgument< ArgType >( title_, args, i, callLoc );
 335                                 if( to->x_ == 0 && to->y_ == 0 && to->z_ == 0 )
 336                                         {
 337                                                 throw Exceptions::CoreOutOfRange( title_, args, i, "The <to> direction is degenerate, that is (0,0,0)." );
 338                                         }
 339
 340                                 gsl_matrix * R = gsl_matrix_alloc( 3, 3 );
 341                                 gsl_vector * t = gsl_vector_alloc( 3 );
 342                                 Helpers::minRotationMatrix3D( Concrete::UnitFloatTriple( from->x_, from->y_, from->z_ ),
 343                                                                                                                                                         Concrete::UnitFloatTriple( to->x_, to->y_, to->z_ ),
 344                                                                                                                                                         R );
 345                                 gsl_vector_set_zero( t );
 346
 347                                 Kernel::ContRef cont = evalState->cont_;
 348                                 cont->takeValue( Kernel::ValueRef( new Lang::Transform3D( R, t ) ),
 349                                                                                                  evalState );
 350
 351                                 gsl_vector_free( t );
 352                                 gsl_matrix_free( R );
 353                         }
 354                 };
 355
 356         }
 357 }
 358
 359 void
 360 Kernel::registerCore_decomp( Kernel::Environment * env )
 361 {
 362         env->initDefineCoreFunction( new Lang::Core_Schur_decomposition( "Schur_decomp" ) );
 363         env->initDefineCoreFunction( new Lang::Core_rotationMapping3D( "rotationMap3D" ) );
 364 }