**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.