CLPR fails for 3d change of basis (precision / epsilon error ?)

Help!
,
1

I’m using: SWI-Prolog version 9.0.4

I want the code to:
Implementation of a 3d change of basis.
This is the formula :

a_M_c = a_M_b * b_M_c

Where the matrices are 4x4 homogeneous matrices where the first 3x3 is the rotation, the last column is translation.

I want to implement it in a CLP way.
Very basically, I want to write it in a way that one of the 3 matrices can be computed if the other 2 are given.

I don’t only represent this as matrices, but more as “3d transform” having 3 translations and 3 rotation angles.

[TransX, TransY, TransZ, Alpha, Beta, Gamma]

For the rotation convention, I use the convention of this formula :

( the second formula, which represents an extrinsic rotation whose (improper) Euler angles are α, β, γ, about axes x, y, z.).
So that i represent the 3x3 rotation only with 3 angles (alpha, beta, gamma).

So my change of basis can also be written as :

TransformProduct = TransLeft * TransRight

But what I’m getting is:
My implementation works when some values are given. But not in all case.
When TransLeft and TransRight are given, this works : I get TransformProduct.
When TransLeft and TransformProduct are given, this works : I get TransRight.
When TransLeft,TransRight, TransformProduct are given, this works : I get True (when it holds, otherwise False).

BUT, suprinsingly, this does NOT work when TransRight and TransformProduct are given, it does not compute TransLeft . I get false instead.

I tought this was a problem of precision / epsilon.
So I changed the constraints to introduce a Epsilon constraint.
And yes, it changed something, but still, I cannot “mimize(Eps)”, otherwise it will return False.

My code looks like this:


:- use_module(library(clpr)).

compose_transform_RzRyRx_GammaBetaAlpha_fulldebug(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, 
            TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight,
            TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd,
            [[RxxLeft, RxyLeft, RxzLeft, TransXLeft], [RyxLeft, RyyLeft, RyzLeft, TransYLeft], [RzxLeft, RzyLeft, RzzLeft, TransZLeft], [0.0, 0.0, 0.0, 1.0]],
            [[RxxRight, RxyRight, RxzRight, TransXRight], [RyxRight, RyyRight, RyzRight, TransYRight], [RzxRight, RzyRight, RzzRight, TransZRight], [0.0, 0.0, 0.0, 1.0]],
            [[RxxProd, RxyProd, RxzProd, TransXProd], [RyxProd, RyyProd, RyzProd, TransYProd], [RzxProd, RzyProd, RzzProd, TransZProd], [0.0, 0.0, 0.0, 1.0]],
            Eps):-            
            {
                    RxxLeft = cos(BetaLeft) * cos(GammaLeft),
                    RxyLeft = sin(AlphaLeft) * sin(BetaLeft) * cos(GammaLeft) - cos(AlphaLeft) * sin(GammaLeft),
                    RxzLeft = cos(AlphaLeft) * sin(BetaLeft) * cos(GammaLeft) + sin(AlphaLeft) * sin(GammaLeft),
                    
                    RyxLeft = cos(BetaLeft) * sin(GammaLeft),
                    RyyLeft = sin(AlphaLeft) * sin(BetaLeft) * sin(GammaLeft) + cos(AlphaLeft) * cos(GammaLeft),
                    RyzLeft = cos(AlphaLeft) * sin(BetaLeft) * sin(GammaLeft) - sin(AlphaLeft) * cos(GammaLeft),
                    
                    RzxLeft = -sin(BetaLeft),
                    RzyLeft = sin(AlphaLeft) * cos(BetaLeft),
                    RzzLeft = cos(AlphaLeft) * cos(BetaLeft),
        
                    RxxRight = cos(BetaRight) * cos(GammaRight),
                    RxyRight = sin(AlphaRight) * sin(BetaRight) * cos(GammaRight) - cos(AlphaRight) * sin(GammaRight),
                    RxzRight = cos(AlphaRight) * sin(BetaRight) * cos(GammaRight) + sin(AlphaRight) * sin(GammaRight),
                    
                    RyxRight = cos(BetaRight) * sin(GammaRight),
                    RyyRight = sin(AlphaRight) * sin(BetaRight) * sin(GammaRight) + cos(AlphaRight) * cos(GammaRight),
                    RyzRight = cos(AlphaRight) * sin(BetaRight) * sin(GammaRight) - sin(AlphaRight) * cos(GammaRight),
                    
                    RzxRight = -sin(BetaRight),
                    RzyRight = sin(AlphaRight) * cos(BetaRight),
                    RzzRight = cos(AlphaRight) * cos(BetaRight),
        
                    RxxProd = RxxLeft * RxxRight + RxyLeft * RyxRight + RxzLeft * RzxRight,
                    RxyProd = RxxLeft * RxyRight + RxyLeft * RyyRight + RxzLeft * RzyRight,
                    RxzProd = RxxLeft * RxzRight + RxyLeft * RyzRight + RxzLeft * RzzRight,
                    TransXProd = RxxLeft * TransXRight + RxyLeft * TransYRight + RxzLeft * TransZRight + TransXLeft,
        
                    RyxProd = RyxLeft * RxxRight + RyyLeft * RyxRight + RyzLeft * RzxRight,
                    RyyProd = RyxLeft * RxyRight + RyyLeft * RyyRight + RyzLeft * RzyRight,
                    RyzProd = RyxLeft * RxzRight + RyyLeft * RyzRight + RyzLeft * RzzRight,
                    TransYProd = RyxLeft * TransXRight + RyyLeft * TransYRight + RyzLeft * TransZRight + TransYLeft,
        
                    RzxProd = RzxLeft * RxxRight + RzyLeft * RyxRight + RzzLeft * RzxRight,
                    RzyProd = RzxLeft * RxyRight + RzyLeft * RyyRight + RzzLeft * RzyRight,
                    RzzProd = RzxLeft * RxzRight + RzyLeft * RyzRight + RzzLeft * RzzRight,
                    TransZProd = RzxLeft * TransXRight + RzyLeft * TransYRight + RzzLeft * TransZRight + TransZLeft,
        
                    Eps >= 0.0,
    
                    %We are supposed to have RxxProd = cos(BetaProd) * cos(GammaProd) but I suspect that it sometimes fails because of rounding errors
                    %Same remark for other values.
                    RxxProdDiff = RxxProd - cos(BetaProd) * cos(GammaProd),
                    RxxProdDiff =< Eps,
                    RxxProdDiff >= - Eps,
    
                    RxyProdDiff = RxyProd - (sin(AlphaProd) * sin(BetaProd) * cos(GammaProd) - cos(AlphaProd) * sin(GammaProd)),
                    RxyProdDiff =< Eps,
                    RxyProdDiff >= - Eps,
                    
                    RxzProdDiff = RxzProd - (cos(AlphaProd) * sin(BetaProd) * cos(GammaProd) + sin(AlphaProd) * sin(GammaProd)),
                    RxzProdDiff =< Eps,
                    RxzProdDiff >= - Eps,
    
                    RyxProdDiff = RyxProd - cos(BetaProd) * sin(GammaProd),
                    RyxProdDiff =< Eps,
                    RyxProdDiff >= - Eps,
    
                    RyyProdDiff = RyyProd - (sin(AlphaProd) * sin(BetaProd) * sin(GammaProd) + cos(AlphaProd) * cos(GammaProd)),
                    RyyProdDiff =< Eps,
                    RyyProdDiff >= - Eps,
                    
                    RyzProdDiff = RyzProd - (cos(AlphaProd) * sin(BetaProd) * sin(GammaProd) - sin(AlphaProd) * cos(GammaProd)),
                    RyzProdDiff =< Eps,
                    RyzProdDiff >= - Eps,
    
                    RzxProdDiff = RzxProd + sin(BetaProd), % = RzxProd - (- sin(BetaProd))
                    RzxProdDiff =< Eps,
                    RzxProdDiff >= - Eps,
    
                    RzyProdDiff = RzyProd - sin(AlphaProd) * cos(BetaProd),
                    RzyProdDiff =< Eps,
                    RzyProdDiff >= - Eps,
    
                    RzzProdDiff = RzzProd - cos(AlphaProd) * cos(BetaProd),
                    RzzProdDiff =< Eps,
                    RzzProdDiff >= - Eps
            }.

compose_transform_RzRyRx_GammaBetaAlpha(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, 
    TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight,
    TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd,
    Eps):-

  compose_transform_RzRyRx_GammaBetaAlpha_fulldebug(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, 
              TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight,
              TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd,
      _, _, _, Eps).

test_compose_transform_RzRyRx_GammaBetaAlpha_1(TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd, Eps):-
    compose_transform_RzRyRx_GammaBetaAlpha(10.0, -20.0, 30.0, 0.5, -0.35, 0.25, 
        -4.5, 23.5, -17.5, -0.45, 0.65, -0.15,
        TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd, 
        Eps).

test_compose_transform_RzRyRx_GammaBetaAlpha_2(TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd, Mleft, Mright, Mprod, Eps):-
    compose_transform_RzRyRx_GammaBetaAlpha_fulldebug(10.0, -20.0, 30.0, 0.5, -0.35, 0.25, 
        -4.5, 23.5, -17.5, -0.45, 0.65, -0.15,
        TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd,
        Mleft, Mright, Mprod, 
        Eps).

test_compose_transform_RzRyRx_GammaBetaAlpha_3(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, Eps):- 
    compose_transform_RzRyRx_GammaBetaAlpha(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, 
        -4.5, 23.5, -17.5, -0.45, 0.65, -0.15,
        0.0855206100177881, 7.4124421153311175, 24.613803224979733, 0.09408983628294156, 0.2864709557112676, 0.44486185098355907,
        Eps).

?- test_compose_transform_RzRyRx_GammaBetaAlpha_1(TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd, Eps), minimize(Eps).
%works

?- test_compose_transform_RzRyRx_GammaBetaAlpha_2(TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd, Mleft, Mright, Mprod, Eps), minimize(Eps).
%works

?- test_compose_transform_RzRyRx_GammaBetaAlpha_3(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, Eps), minimize(Eps).
%fails. Because of rounding ?

?- test_compose_transform_RzRyRx_GammaBetaAlpha_3(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, Eps).
%This returns an answer space. So this is likely a proplem of precision ? 

?- test_compose_transform_RzRyRx_GammaBetaAlpha_3(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, 0.000000001).
%This returns an answer space. So this is likely a proplem of precision ?

How can I solve this issue ?

And / or how can I get a result inside the returned space of

?- test_compose_transform_RzRyRx_GammaBetaAlpha_3(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, 0.000000001).

I can also send you the full code if you want.

Thanks in advance.

2

According to https://eu.swi-prolog.org/pldoc/man?section=clpqr, clpr currently has no maintainer. While I haven’t used it and don’t understand any of its internals, let me take a stab.

Due to rounding and conversion errors, floating point arithmetic is mathematically unsound. It looks like clpr doesn’t deal with its illogical behaviour. For example given the set of constraints:

{Z = X*Y, X = Z/Y, Y = Z/X}

and any two of the three values, it should derive the third. But:

?- {Z = X*Y, X = Z/Y, Y = Z/X}, X=0.1, Y= 0.2.
false.

Using additional variables, you can see why:

?- {Z = X*Y, X1 = Z/Y, Y1 = Z/X}, X=0.1, Y= 0.2.
Z = 0.020000000000000004,
X = 0.1,
Y = 0.2,
X1 = 0.10000000000000002,
Y1 = 0.20000000000000004 .

Clearly X =\= X1 and Y \= Y1 due to conversion errors and/or unsound floating point arithmetic.

I don’t quite understand what you’re doing with Eps. It looks like you’re constraining it be be >= to the largest error as the values of the other variables are resolved, and then setting it to the minimum, which should be the largest error. It’s not obvious to me why this last step fails.

It may be worth looking at “minimizing” a total error expression instead, e.g.,

 {ErrorF = RxxProdDiff^2 + RxyProdDif^2 + ... + RzzProdDiff^2}

and then minimize(ErrorF). But I’m not sure that will solve the problem and it seems like a lot of extra work to deal with “faulty” arithmetic.

I’m responsible for pack(clpBNR) https://eu.swi-prolog.org/pack/list?p=clpBNR which implements mathematically sound arithmetic over reals based on interval arithmetic. So:

?- {Z == X*Y, X == Z/Y, Y == Z/X}, X=0.1, Y= 0.2.
X = 0.1,
Y = 0.2,
Z:: 0.0200000000000000... .

A possible downside is you may be left with “fuzzy” values but that’s really the best you can do given that the set of reals is not a finite domain. (There are an infinite number of real values between two adjacent floating point values.)

If you think clpBNR might be a fit for what you’re trying to do, I’ll try to answer any questions you might have.

HTH.

3

Dear Ridgeworks, thank you very much for your help. Let me give you some feedback.

Eps stands for epsilon. This is an admissable rounding error. It must be positive. So that I can enclose every error between -Eps and Eps. Like -Eps =< ComputedValue - TheSameValueWithDifferentRounding =< Eps. In your example, this would be -Eps =< Y1-Y =< Eps, which is true for instance for Eps = 0.000001. And then I minimize Eps so that Y1is as close as possible to Y. Sorry if this is not orthodox.

I tried your proposal of minimizing the sum of squared errors.
I removed everything dealing with Eps and modified to :

 ErrorProd = pow(RxxProdDiff,2) + pow(RxyProdDiff,2) + pow(RxzProdDiff,2) +
                         pow(RyxProdDiff,2) + pow(RyyProdDiff,2) + pow(RyzProdDiff,2) +
                         pow(RzxProdDiff,2) + pow(RzyProdDiff,2) + pow(RzzProdDiff,2)
 
 
     }, minimize(ErrorProd).

And I also tried RxxProdDiff*RxxProdDiffrather than pow(RxxProdDiff,2).

Saddly this is even worst, because now NO test succeed …

Any other proposal ?

I might give a try with your librairy but I don’t have the time today.

What do you mean by “fuzzy” values ?
It is something to deal with your
Z:: 0.0200000000000000... ?
Is there a way I can turn it to a real at the end ? Like truncated or whatever (I don’t need angstrom precision).

Kind Regards

4

I think I understand the jist of what you’re trying to do, but I don’t think that’s what’s actually happening with your code. The clpr doc for minimize/1 says:

Minimizes Expression within the current constraint store. This is the same as computing the infimum and equating the expression to that infimum.

so rather than minimizing (which currently fails) why don’t we just calculate the infimum of Eps:

?- test_compose_transform_RzRyRx_GammaBetaAlpha_3(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft, Eps), inf(Eps,EpsInf).
EpsInf = 0.0,
{_A=<0.0, _B>=0.0, _= -0.5*_B-0.5*_A, _C=<0.0, _=_C+_B-_A, _C+_B-_A>=0.0, _= -_C-0.5*_B+0.5*_A, _D=<0.0, 
....

so the “calculated infinium” of Eps is 0.0. Given any errors in the calculation at all, setting Eps to 0.0, as would be done in minimize/1, must fail, no?

I would guess that the reason the infinium of Eps is 0.0 is that the only active constraint after test_compose_transform_RzRyRx_GammaBetaAlpha_3 is { Eps >= 0 }; all the others are passive constraints waiting for some condition (see isolation axioms) to set variables to values. But I’m not an expert on clpr so there well may be some other explanation.

Many ways, e.g., set the “fuzzy” value to it’s midpoint:

?- {Z == X*Y, X == Z/Y, Y == Z/X}, X=0.1, Y= 0.2, Z is midpoint(Z).
Z = 0.020000000000000004,
X = 0.1,
Y = 0.2.

But you wouldn’t want to set it to a float until “the end”; otherwise you’re back in unsound arithmetic land (as the floating point result indicates).

Also don’t equate “real” to “floating point” - the first is mathematics, the second is computer science. The set of IEEE floating point numbers is a (vanishingly small) subset of real numbers.

5

I think you are running up against two limitations of clpr:

  1. Non-linear constraints are passive until another constraint is applied to make it linear.

  2. Floating point arithmetic is unsound so two expressions which are mathematically equivalent do not evaluate as equal.

It’s not clear to me how to overcome these limitations for your problem. Perhaps someone more familiar with clpr will provide some suggestions.

For what it’s worth, I implemented a solution in clpbnr both as an example for my own testing purposes and to verify there were no hidden "gotcha"s in the math as written. This was fairly straight forward but exposed a couple of issues. My version of the predicate has the signature:

constrain_transform_RzRyRx_GammaBetaAlpha(
    TransXLeft,  TransYLeft,  TransZLeft,  AlphaLeft,  BetaLeft,  GammaLeft, 
    TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight,
    TransXProd,  TransYProd,  TransZProd,  AlphaProd,  BetaProd,  GammaProd
   )

The first issue was that if the angles are not constrained to be in the range (-pi,pi) there is more than one solution. Also clpbnr reals include ±inf, so the Trans* values must be constrained to be finite. (I used the default values of ±1e16.)

Given these additional constraints, the results of your 2 test cases:

?- test_constrain_transform_RzRyRx_GammaBetaAlpha_1(TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd).
TransXProd:: 0.0855206100178...,
TransYProd:: 7.412442115331...,
TransZProd:: 24.6138032249797...,
AlphaProd:: 0.09408983628294...,
BetaProd:: 0.28647095571126...,
GammaProd:: 0.44486185098356... .

?- test_constrain_transform_RzRyRx_GammaBetaAlpha_3(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft).
TransXLeft::real(8.259960687502137, 11.76514674311086),
TransYLeft::real(-23.3238902059514, -16.745825674489407),
TransZLeft::real(28.755923227723528, 31.225460265239974),
AlphaLeft:: 0.5...,
BetaLeft:: -0.3...,
GammaLeft::real(0.17167301509126393, 0.3309015813718558).

You’ve probably noticed that the answers for the second test case (test_constrain_transform_RzRyRx_GammaBetaAlpha_3) contain the expected result but the intervals are wide (i.e., not precise). For this particular problem constraint propagation was slow, activating an internal throttling mechanism so as to not give the appearance of non-termination. This mechanism is a heuristic with a user definable threshold parameter. So one way of getting a more precise result is by increasing the threshold, e.g.,

?- set_prolog_flag(clpBNR_iteration_limit,20000).
true.

?- test_constrain_transform_RzRyRx_GammaBetaAlpha_3(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft).
TransXLeft:: 10.000...,
TransYLeft:: -20.000...,
TransZLeft:: 30.0000...,
AlphaLeft:: 0.50000...,
BetaLeft:: -0.35000...,
GammaLeft:: 0.25000... .

Another way is to “search” for a better result using solve/1, analogous to labeling in clpfd:

?- test_constrain_transform_RzRyRx_GammaBetaAlpha_3(TransXLeft, TransYLeft, TransZLeft, AlphaLeft, BetaLeft, GammaLeft), solve([AlphaLeft, BetaLeft, GammaLeft]).
TransXLeft:: 10.000000000000...,
TransYLeft:: -20.000000000000...,
TransZLeft:: 30.000000000000...,
AlphaLeft:: 0.5000000000000...,
BetaLeft:: -0.3500000000000...,
GammaLeft:: 0.2500000000000... ;
false.

producing a fairly precise result. I think it’s effective for this problem since the search fairly quickly prunes away parts of the solution space where constraint propagation is slow.

I also added a third test case calculating the Trans*Right from Trans*Left and Trans*Prod:

test_constrain_transform_RzRyRx_GammaBetaAlpha_4(TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight):- 
    constrain_transform_RzRyRx_GammaBetaAlpha(
        10.0, -20.0, 30.0, 0.5, -0.35, 0.25, 
        TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight,
        0.0855206100177881, 7.4124421153311175, 24.613803224979733, 0.09408983628294156, 0.2864709557112676, 0.44486185098355907 % TransXProd, TransYProd, TransZProd, AlphaProd, BetaProd, GammaProd,
        ).

with the following results:

?- test_constrain_transform_RzRyRx_GammaBetaAlpha_4(TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight).
TransXRight:: -4.500000000000...,
TransYRight:: 23.500000000000...,
TransZRight:: -17.500000000000...,
AlphaRight::real(-0.6124042349089082, -0.32394880165368584),
BetaRight::real(0.5961825023418276, 0.7054558659262122),
GammaRight::real(-0.21794118794715925, -0.09054657629573598).

?- set_prolog_flag(clpBNR_iteration_limit,20000).
true.

?- test_constrain_transform_RzRyRx_GammaBetaAlpha_4(TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight).
TransXRight:: -4.500000000000...,
TransYRight:: 23.500000000000...,
TransZRight:: -17.500000000000...,
AlphaRight:: -0.4500...,
BetaRight:: 0.65000...,
GammaRight:: -0.15000... .

?- test_constrain_transform_RzRyRx_GammaBetaAlpha_4(TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight), solve([AlphaRight, BetaRight, GammaRight]).
TransXRight:: -4.500000000000...,
TransYRight:: 23.500000000000...,
TransZRight:: -17.500000000000...,
AlphaRight:: -0.4500000000000...,
BetaRight:: 0.6500000000000...,
GammaRight:: -0.1500000000000... ;
false.

I did wonder about constraints that didn’t appear “symmetrical” with respect to Right and Left, e.g.,

  TransXProd == RxxLeft * TransXRight + RxyLeft * TransYRight + RxzLeft * TransZRight + TransXLeft

but don’t understand enough about the problem to comment further. Sometimes adding redundant constraints can improve the performance of low level constraint propagation.

`clpbnr` version (refactors some common code into `rotate_matrix/4`: 
constrain_transform_RzRyRx_GammaBetaAlpha(
	 TransXLeft,  TransYLeft,  TransZLeft,  AlphaLeft,  BetaLeft,  GammaLeft,
	 TransXRight, TransYRight, TransZRight, AlphaRight, BetaRight, GammaRight,
	 TransXProd,  TransYProd,  TransZProd,  AlphaProd,  BetaProd,  GammaProd
	) :-
	
	[TransXLeft,  TransYLeft,  TransZLeft,
	 TransXRight, TransYRight, TransZRight,
	 TransXProd,  TransYProd,  TransZProd
	]::real,      % ensure finite

	rotate_matrix([[RxxLeft, RxyLeft, RxzLeft], [RyxLeft, RyyLeft, RyzLeft], [RzxLeft, RzyLeft, RzzLeft]], 
	              [AlphaLeft, BetaLeft, GammaLeft]
	             ),

	rotate_matrix([[RxxRight, RxyRight, RxzRight], [RyxRight, RyyRight, RyzRight], [RzxRight, RzyRight, RzzRight]], 
	              [AlphaRight, BetaRight, GammaRight]
	             ),

	rotate_matrix([[RxxProd, RxyProd, RxzProd], [RyxProd, RyyProd, RyzProd], [RzxProd, RzyProd, RzzProd]], 
	              [AlphaProd, BetaProd, GammaProd]
	             ),

	{ RxxProd == RxxLeft * RxxRight + RxyLeft * RyxRight + RxzLeft * RzxRight,
	  RxyProd == RxxLeft * RxyRight + RxyLeft * RyyRight + RxzLeft * RzyRight,
	  RxzProd == RxxLeft * RxzRight + RxyLeft * RyzRight + RxzLeft * RzzRight,
	  TransXProd == RxxLeft * TransXRight + RxyLeft * TransYRight + RxzLeft * TransZRight + TransXLeft,

	  RyxProd == RyxLeft * RxxRight + RyyLeft * RyxRight + RyzLeft * RzxRight,
	  RyyProd == RyxLeft * RxyRight + RyyLeft * RyyRight + RyzLeft * RzyRight,
	  RyzProd == RyxLeft * RxzRight + RyyLeft * RyzRight + RyzLeft * RzzRight,
	  TransYProd == RyxLeft * TransXRight + RyyLeft * TransYRight + RyzLeft * TransZRight + TransYLeft,

	  RzxProd == RzxLeft * RxxRight + RzyLeft * RyxRight + RzzLeft * RzxRight,
	  RzyProd == RzxLeft * RxyRight + RzyLeft * RyyRight + RzzLeft * RzyRight,
	  RzzProd == RzxLeft * RxzRight + RzyLeft * RyzRight + RzzLeft * RzzRight,
	  TransZProd == RzxLeft * TransXRight + RzyLeft * TransYRight + RzzLeft * TransZRight + TransZLeft
	}.


rotate_matrix([[Rxx, Rxy, Rxz], [Ryx, Ryy, Ryz], [Rzx, Rzy, Rzz]], 
	          [Alpha, Beta, Gamma] 
	         ):-
	[Alpha, Beta, Gamma]::real(-pi/2,pi/2),
	{ Rxx == cos(Beta) * cos(Gamma),
	  Rxy == sin(Alpha) * sin(Beta) * cos(Gamma) - cos(Alpha) * sin(Gamma),
	  Rxz == cos(Alpha) * sin(Beta) * cos(Gamma) + sin(Alpha) * sin(Gamma),

	  Ryx == cos(Beta) * sin(Gamma),
	  Ryy == sin(Alpha) * sin(Beta) * sin(Gamma) + cos(Alpha) * cos(Gamma),
	  Ryz == cos(Alpha) * sin(Beta) * sin(Gamma) - sin(Alpha) * cos(Gamma),

	  Rzx == -sin(Beta),
	  Rzy == sin(Alpha) * cos(Beta),
	  Rzz == cos(Alpha) * cos(Beta)
	}.
2 Likes
6

Thank you Ridgeworks. I tried myself this morning (before your post) and arrived to the almost the same solution / same results as you, using solve (but your code is better factorized and clean, I am still very new to Prolog…). I did not noticed the set_prolog_flag(clpBNR_iteration_limit,20000). . When should I use the solve or the clpBNR_iteration_limit?

Remark : I think you have restricted the angles to much. I changed to

   %[Alpha, Beta, Gamma]::real(-pi/2,pi/2), %<- proposal from Ridgeworks. But this is too limiting IMHO. BB 20230620
   [Alpha, Gamma]::real(-pi,pi), %Looking at : https://en.wikipedia.org/wiki/Euler_angles#Signs,_ranges_and_conventions
   Beta::real(-pi/2,pi/2), %Looking at : https://en.wikipedia.org/wiki/Euler_angles#Signs,_ranges_and_conventions

I was expecting then more solutions, but no… Maybe I should find and try particular examples with particular angles …

7

I wish I had a good answer. The rate at which intervals narrow is dependant on the particular constraints being applied so it’s generally impossible to predict. It’s a fixed point iteration so it can take a while for a 64 bit floating point bound to stop changing. In many situations constraints are applied incrementally so you don’t want to work too hard at any particular point if a future constraint makes the whole job easier. So I think it’s probably best to use solve (with the default iteration limit) in most situations.

I think you’re right. When I set all three angles to [-pi,pi], I was getting two solutions with solve. [-pi/2,pi/2] for all yielded in just one, but I agree that’s too restrictive (see Wikipedia page). A range of [-pi,pi] for Alpha and Gamma is correct, but appears to require the use of solve (still 1 solution), whereas a range of, for example, [-0.999*pi,0.999*pi] will also narrow via fixed point convergence. This just reinforces what I said above about how difficult it is to predict which approach (solve vs. extended iteration) will be the most effective in any particular situation.

Anyway, I think you’ve gotten enough working that you can sort out the details.