1 /* This file is part of Shapes.
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
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.
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/>.
16 * Copyright 2008 Henrik Tidefelt
21 #include "shapescore.h"
23 #include "shapesexceptions.h"
26 #include "continuations.h"
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>
36 using namespace Shapes
;
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" );
47 void minRotationMatrix3D( const Concrete::UnitFloatTriple
& from
, const Concrete::UnitFloatTriple
& to
, gsl_matrix
* dst_3_3
);
55 class Core_Schur_decomposition
: public Lang::CoreFunction
58 Core_Schur_decomposition( const char * title
)
59 : CoreFunction( title
, new Kernel::EvaluatedFormals( Ast::FileID::build_internal( title
), true ) )
61 formals_
->appendEvaluatedCoreFormal( "tf", Kernel::THE_SLOT_VARIABLE
);
62 formals_
->appendEvaluatedCoreFormal( "rank", Kernel::THE_VOID_VARIABLE
);
63 formals_
->appendEvaluatedCoreFormal( "canonical", Kernel::THE_FALSE_VARIABLE
);
66 call( Kernel::EvalState
* evalState
, Kernel::Arguments
& args
, const Ast::SourceLocation
& callLoc
) const
68 args
.applyDefaults( );
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
>( ) )
80 throw Exceptions::CoreOutOfRange( title_
, args
, argsi
, "Rank is negative." );
85 typedef const Lang::Boolean FlagType
;
86 bool canonical
= Helpers::down_cast_CoreArgument
< FlagType
>( title_
, args
, argsi
, callLoc
)->val_
;
91 typedef const Lang::Transform2D ArgType
;
92 RefCountPtr
< ArgType
> tf
= Helpers::try_cast_CoreArgument
< ArgType
>( args
.getValue( argsi
) );
95 throw Exceptions::CoreOutOfRange( title_
, args
, argsi
, "Rank exceeds dimension." );
97 Helpers::Schur_decomposition_helper_2D( evalState
, tf
, rank
, canonical
, callLoc
);
100 catch( const NonLocalExit::NotThisType
& ball
)
102 /* Wrong type; never mind!.. but see below!
108 typedef const Lang::Transform3D ArgType
;
109 RefCountPtr
< ArgType
> tf
= Helpers::try_cast_CoreArgument
< ArgType
>( args
.getValue( argsi
) );
112 throw Exceptions::CoreOutOfRange( title_
, args
, argsi
, "Rank exceeds dimension." );
114 Helpers::Schur_decomposition_helper_3D( evalState
, tf
, rank
, canonical
, callLoc
);
117 catch( const NonLocalExit::NotThisType
& ball
)
119 /* Wrong type; never mind!.. but see below!
123 throw Exceptions::CoreTypeMismatch( callLoc
, title_
, args
, 0, Helpers::typeSetString( Lang::Transform2D::staticTypeName( ), Lang::Transform3D::staticTypeName( ) ) );
134 Schur_decomposition_helper_2D( Kernel::EvalState
* evalState
, const RefCountPtr
< const Lang::Transform2D
> & tf
, int rank
, bool canonical
, const Ast::SourceLocation
& callLoc
)
136 throw Exceptions::NotImplemented( "Schur_decomposition_helper_2D" );
140 Schur_decomposition_helper_3D( Kernel::EvalState
* evalState
, const RefCountPtr
< const Lang::Transform3D
> & tf
, int rank
, bool canonical
, const Ast::SourceLocation
& callLoc
)
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 )
153 throw Exceptions::ExternalError( "gsl_eigen_nonsymm_Z failed. (This is one of those \"rare occations\"...)" );
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.
158 if( fabs( gsl_matrix_get( L
, 1, 0 ) ) < fabs( gsl_matrix_get( L
, 2, 1 ) ) )
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
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.)
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 )
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.
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( ) ),
186 /* The only thing we can do is to ensure that the Q is special.
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_
);
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
);
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
);
208 gsl_vector_set_zero( Qp
);
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.
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
);
226 for( size_t i
= rank
; i
< N
; ++i
)
228 gsl_vector_set( sigma
, i
, 0 );
233 /* A negative value of rank means that the rank shall be determined automatically.
235 const double lim
= 1e-2 * gsl_vector_get( sigma
, 0 );
236 for( size_t i
= 0; i
< N
; ++i
)
238 if( fabs( gsl_vector_get( sigma
, i
) ) < lim
)
240 gsl_vector_set( sigma
, i
, 0 );
244 gsl_linalg_SV_solve( A
, V
, sigma
, QTp
, Qp
);
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
);
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
) ) );
257 Kernel::ContRef cont
= evalState
->cont_
;
258 cont
->takeValue( Schur_decomposition_resultFactory
.build( ),
265 Helpers::minRotationMatrix3D( const Concrete::UnitFloatTriple
& from
, const Concrete::UnitFloatTriple
& to
, gsl_matrix
* dst_3_3
)
267 /* The rotation maps:
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
);
300 catch( const NonLocalExit::CrossDirectionOfParallel
& ball
)
302 gsl_matrix_set_identity( dst_3_3
);
310 class Core_rotationMapping3D
: public Lang::CoreFunction
313 Core_rotationMapping3D( const char * title
)
314 : CoreFunction( title
, new Kernel::EvaluatedFormals( Ast::FileID::build_internal( title
), true ) )
316 formals_
->appendEvaluatedCoreFormal( "from", Kernel::THE_SLOT_VARIABLE
);
317 formals_
->appendEvaluatedCoreFormal( "to", Kernel::THE_SLOT_VARIABLE
);
320 call( Kernel::EvalState
* evalState
, Kernel::Arguments
& args
, const Ast::SourceLocation
& callLoc
) const
322 args
.applyDefaults( );
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 )
330 throw Exceptions::CoreOutOfRange( title_
, args
, i
, "The <from> direction is degenerate, that is (0,0,0)." );
334 RefCountPtr
< ArgType
> to
= Helpers::down_cast_CoreArgument
< ArgType
>( title_
, args
, i
, callLoc
);
335 if( to
->x_
== 0 && to
->y_
== 0 && to
->z_
== 0 )
337 throw Exceptions::CoreOutOfRange( title_
, args
, i
, "The <to> direction is degenerate, that is (0,0,0)." );
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_
),
345 gsl_vector_set_zero( t
);
347 Kernel::ContRef cont
= evalState
->cont_
;
348 cont
->takeValue( Kernel::ValueRef( new Lang::Transform3D( R
, t
) ),
351 gsl_vector_free( t
);
352 gsl_matrix_free( R
);
360 Kernel::registerCore_decomp( Kernel::Environment
* env
)
362 env
->initDefineCoreFunction( new Lang::Core_Schur_decomposition( "Schur_decomp" ) );
363 env
->initDefineCoreFunction( new Lang::Core_rotationMapping3D( "rotationMap3D" ) );