OrienRepr.EulerAngle
Section EulerAngle24.
Variable θ1 θ2 θ3 : R.
Notation c1 := (cos θ1). Notation s1 := (sin θ1).
Notation c2 := (cos θ2). Notation s2 := (sin θ2).
Notation c3 := (cos θ3). Notation s3 := (sin θ3).
Variable θ1 θ2 θ3 : R.
Notation c1 := (cos θ1). Notation s1 := (sin θ1).
Notation c2 := (cos θ2). Notation s2 := (sin θ2).
Notation c3 := (cos θ3). Notation s3 := (sin θ3).
关于0点的线性化
Section LinearizationCondition_at_Zero.
Definition LinearizationCondition : Prop :=
(s1 = θ1 /\ s2 = θ2 /\ s3 = θ3 /\
c1 = 1 /\ c2 = 1 /\ c3 = 1 /\
s1 * s2 = 0 /\ s1 * s3 = 0 /\ s2 * s3 = 0 /\
s2 * s1 = 0 /\ s3 * s1 = 0 /\ s3 * s2 = 0)%R.
End LinearizationCondition_at_Zero.
Definition LinearizationCondition : Prop :=
(s1 = θ1 /\ s2 = θ2 /\ s3 = θ3 /\
c1 = 1 /\ c2 = 1 /\ c3 = 1 /\
s1 * s2 = 0 /\ s1 * s3 = 0 /\ s2 * s3 = 0 /\
s2 * s1 = 0 /\ s3 * s1 = 0 /\ s3 * s2 = 0)%R.
End LinearizationCondition_at_Zero.
1. Body-three, 123
Definition B123 : mat 3 3 :=
l2m
[[c2 * c3; -c2 * s3; s2];
[s1 * s2 * c3 + s3 * c1; - s1 * s2 * s3 + c3 * c1; - s1 * c2];
[-c1 * s2 * c3 + s3 * s1; c1 * s2 * s3 + c3 * s1; c1 * c2]]%R.
Theorem B123_spec : B123 = Rx θ1 * Ry θ2 * Rz θ3.
Proof. meq; ring. Qed.
l2m
[[c2 * c3; -c2 * s3; s2];
[s1 * s2 * c3 + s3 * c1; - s1 * s2 * s3 + c3 * c1; - s1 * c2];
[-c1 * s2 * c3 + s3 * s1; c1 * s2 * s3 + c3 * s1; c1 * c2]]%R.
Theorem B123_spec : B123 = Rx θ1 * Ry θ2 * Rz θ3.
Proof. meq; ring. Qed.
B123 is a member of SO3
Lemma B123_SOnP : SOnP B123.
Proof.
rewrite B123_spec. apply SOnP_mmul. apply SOnP_mmul.
apply Rx_SOnP. apply Ry_SOnP. apply Rz_SOnP.
Qed.
Section verify_orth_keep_cross.
Fact orth_keep_cross_c1c2 : B123&1 \x B123&2 = B123&3.
Proof.
apply SO3_v3cross_r12.
rewrite mcol_eq_mtrans.
apply SOnP_mtrans. apply B123_SOnP.
Qed.
End verify_orth_keep_cross.
Definition B123_Linear : mat 3 3 :=
l2m
[[1; -θ3; θ2];
[θ3; 1; -θ1];
[-θ2; θ1; 1]]%R.
Theorem B123_Linear_spec : LinearizationCondition -> B123 = B123_Linear.
Proof.
intros.
destruct H as
[Hs1 [Hs2 [Hs3 [Hc1 [Hc2 [Hc3 [H12 [H13 [H23 [H21 [H31 H32]]]]]]]]]]].
unfold B123, B123_Linear.
rewrite ?Hc1,?Hc2,?Hc3. meq; ring_simplify; auto; try lra.
rewrite associative. rewrite H23. lra.
Qed.
Proof.
rewrite B123_spec. apply SOnP_mmul. apply SOnP_mmul.
apply Rx_SOnP. apply Ry_SOnP. apply Rz_SOnP.
Qed.
Section verify_orth_keep_cross.
Fact orth_keep_cross_c1c2 : B123&1 \x B123&2 = B123&3.
Proof.
apply SO3_v3cross_r12.
rewrite mcol_eq_mtrans.
apply SOnP_mtrans. apply B123_SOnP.
Qed.
End verify_orth_keep_cross.
Definition B123_Linear : mat 3 3 :=
l2m
[[1; -θ3; θ2];
[θ3; 1; -θ1];
[-θ2; θ1; 1]]%R.
Theorem B123_Linear_spec : LinearizationCondition -> B123 = B123_Linear.
Proof.
intros.
destruct H as
[Hs1 [Hs2 [Hs3 [Hc1 [Hc2 [Hc3 [H12 [H13 [H23 [H21 [H31 H32]]]]]]]]]]].
unfold B123, B123_Linear.
rewrite ?Hc1,?Hc2,?Hc3. meq; ring_simplify; auto; try lra.
rewrite associative. rewrite H23. lra.
Qed.
2. Body-three, 231
Definition B231 : mat 3 3 :=
l2m
[[c1 * c2; - c1 * s2 * c3 + s3 * s1; c1 * s2 * s3 + c3 * s1];
[s2; c2 * c3; - c2 * s3];
[- s1 * c2; s1 * s2 * c3 + s3 * c1; - s1 * s2 * s3 + c3 * c1]]%R.
Theorem B231_spec : B231 = Ry θ1 * Rz θ2 * Rx θ3.
Proof. meq; ring. Qed.
l2m
[[c1 * c2; - c1 * s2 * c3 + s3 * s1; c1 * s2 * s3 + c3 * s1];
[s2; c2 * c3; - c2 * s3];
[- s1 * c2; s1 * s2 * c3 + s3 * c1; - s1 * s2 * s3 + c3 * c1]]%R.
Theorem B231_spec : B231 = Ry θ1 * Rz θ2 * Rx θ3.
Proof. meq; ring. Qed.
3. Body-three, 312
Definition B312 : mat 3 3 :=
l2m
[[- s1 * s2 * s3 + c3 * c1; - s1 * c2; s1 * s2 * c3 + s3 * c1];
[c1 * s2 * s3 + c3 * s1; c1 * c2; - c1 * s2 * c3 + s3 * s1];
[- c2 * s3; s2; c2 * c3]]%R.
Theorem B312_spec : B312 = Rz θ1 * Rx θ2 * Ry θ3.
Proof. meq; ring. Qed.
l2m
[[- s1 * s2 * s3 + c3 * c1; - s1 * c2; s1 * s2 * c3 + s3 * c1];
[c1 * s2 * s3 + c3 * s1; c1 * c2; - c1 * s2 * c3 + s3 * s1];
[- c2 * s3; s2; c2 * c3]]%R.
Theorem B312_spec : B312 = Rz θ1 * Rx θ2 * Ry θ3.
Proof. meq; ring. Qed.
4. Body-three, 132
Definition B132 : mat 3 3 :=
l2m
[[c2 * c3; - s2; c2 * s3];
[c1 * s2 * c3 + s3 * s1; c1 * c2; c1 * s2 * s3 - c3 * s1];
[s1 * s2 * c3 - s3 * c1; s1 * c2; s1 * s2 * s3 + c3 * c1]]%R.
Theorem B132_spec : B132 = Rx θ1 * Rz θ2 * Ry θ3.
Proof. meq; ring. Qed.
l2m
[[c2 * c3; - s2; c2 * s3];
[c1 * s2 * c3 + s3 * s1; c1 * c2; c1 * s2 * s3 - c3 * s1];
[s1 * s2 * c3 - s3 * c1; s1 * c2; s1 * s2 * s3 + c3 * c1]]%R.
Theorem B132_spec : B132 = Rx θ1 * Rz θ2 * Ry θ3.
Proof. meq; ring. Qed.
5. Body-three, 213
Definition B213 : mat 3 3 :=
l2m
[[s1 * s2 * s3 + c3 * c1; s1 * s2 * c3 - s3 * c1; s1 * c2];
[c2 * s3; c2 * c3; - s2];
[c1 * s2 * s3 - c3 * s1; c1 * s2 * c3 + s3 * s1; c1 * c2]]%R.
Theorem B213_spec : B213 = Ry θ1 * Rx θ2 * Rz θ3.
Proof. meq; ring. Qed.
l2m
[[s1 * s2 * s3 + c3 * c1; s1 * s2 * c3 - s3 * c1; s1 * c2];
[c2 * s3; c2 * c3; - s2];
[c1 * s2 * s3 - c3 * s1; c1 * s2 * c3 + s3 * s1; c1 * c2]]%R.
Theorem B213_spec : B213 = Ry θ1 * Rx θ2 * Rz θ3.
Proof. meq; ring. Qed.
6. Body-three, 321
Definition B321 : mat 3 3 :=
l2m
[[c1 * c2; c1 * s2 * s3 - c3 * s1; c1 * s2 * c3 + s3 * s1];
[s1 * c2; s1 * s2 * s3 + c3 * c1; s1 * s2 * c3 - s3 * c1];
[- s2; c2 * s3; c2 * c3]]%R.
Theorem B321_spec : B321 = Rz θ1 * Ry θ2 * Rx θ3.
Proof. meq; ring. Qed.
l2m
[[c1 * c2; c1 * s2 * s3 - c3 * s1; c1 * s2 * c3 + s3 * s1];
[s1 * c2; s1 * s2 * s3 + c3 * c1; s1 * s2 * c3 - s3 * c1];
[- s2; c2 * s3; c2 * c3]]%R.
Theorem B321_spec : B321 = Rz θ1 * Ry θ2 * Rx θ3.
Proof. meq; ring. Qed.
7. Body-two, 121
Definition B121 : mat 3 3 :=
l2m
[[c2; s2 * s3; s2 * c3];
[s1 * s2; - s1 * c2 * s3 + c3 * c1; - s1 * c2 * c3 - s3 * c1];
[- c1 * s2; c1 * c2 * s3 + c3 * s1; c1 * c2 * c3 - s3 * s1]]%R.
Theorem B121_spec : B121 = Rx θ1 * Ry θ2 * Rx θ3.
Proof. meq; ring. Qed.
l2m
[[c2; s2 * s3; s2 * c3];
[s1 * s2; - s1 * c2 * s3 + c3 * c1; - s1 * c2 * c3 - s3 * c1];
[- c1 * s2; c1 * c2 * s3 + c3 * s1; c1 * c2 * c3 - s3 * s1]]%R.
Theorem B121_spec : B121 = Rx θ1 * Ry θ2 * Rx θ3.
Proof. meq; ring. Qed.
8. Body-two, 131
Definition B131 : mat 3 3 :=
l2m
[[c2; - s2 * c3; s2 * s3];
[c1 * s2; c1 * c2 * c3 - s3 * s1; - c1 * c2 * s3 - c3 * s1];
[s1 * s2; s1 * c2 * c3 + s3 * c1; - s1 * c2 * s3 + c3 * c1]]%R.
Theorem B131_spec : B131 = Rx θ1 * Rz θ2 * Rx θ3.
Proof. meq; ring. Qed.
l2m
[[c2; - s2 * c3; s2 * s3];
[c1 * s2; c1 * c2 * c3 - s3 * s1; - c1 * c2 * s3 - c3 * s1];
[s1 * s2; s1 * c2 * c3 + s3 * c1; - s1 * c2 * s3 + c3 * c1]]%R.
Theorem B131_spec : B131 = Rx θ1 * Rz θ2 * Rx θ3.
Proof. meq; ring. Qed.
9. Body-two, 212
Definition B212 : mat 3 3 :=
l2m
[[- s1 * c2 * s3 + c3 * c1; s1 * s2; s1 * c2 * c3 + s3 * c1];
[s2 * s3; c2; - s2 * c3];
[- c1 * c2 * s3 - c3 * s1; c1 * s2; c1 * c2 * c3 - s3 * s1]]%R.
Theorem B212_spec : B212 = Ry θ1 * Rx θ2 * Ry θ3.
Proof. meq; ring. Qed.
l2m
[[- s1 * c2 * s3 + c3 * c1; s1 * s2; s1 * c2 * c3 + s3 * c1];
[s2 * s3; c2; - s2 * c3];
[- c1 * c2 * s3 - c3 * s1; c1 * s2; c1 * c2 * c3 - s3 * s1]]%R.
Theorem B212_spec : B212 = Ry θ1 * Rx θ2 * Ry θ3.
Proof. meq; ring. Qed.
10. Body-two, 232
Definition B232 : mat 3 3 :=
l2m
[[c1 * c2 * c3 - s3 * s1; - c1 * s2; c1 * c2 * s3 + c3 * s1];
[s2 * c3; c2; s2 * s3];
[- s1 * c2 * c3 - s3 * c1; s1 * s2; - s1 * c2 * s3 + c3 * c1]]%R.
Theorem B232_spec : B232 = Ry θ1 * Rz θ2 * Ry θ3.
Proof. meq; ring. Qed.
l2m
[[c1 * c2 * c3 - s3 * s1; - c1 * s2; c1 * c2 * s3 + c3 * s1];
[s2 * c3; c2; s2 * s3];
[- s1 * c2 * c3 - s3 * c1; s1 * s2; - s1 * c2 * s3 + c3 * c1]]%R.
Theorem B232_spec : B232 = Ry θ1 * Rz θ2 * Ry θ3.
Proof. meq; ring. Qed.
11. Body-two, 313
Definition B313 : mat 3 3 :=
l2m
[[- s1 * c2 * s3 + c3 * c1; - s1 * c2 * c3 - s3 * c1; s1 * s2];
[c1 * c2 * s3 + c3 * s1; c1 * c2 * c3 - s3 * s1; - c1 * s2];
[s2 * s3; s2 * c3; c2]]%R.
Theorem B313_spec : B313 = Rz θ1 * Rx θ2 * Rz θ3.
Proof. meq; ring. Qed.
l2m
[[- s1 * c2 * s3 + c3 * c1; - s1 * c2 * c3 - s3 * c1; s1 * s2];
[c1 * c2 * s3 + c3 * s1; c1 * c2 * c3 - s3 * s1; - c1 * s2];
[s2 * s3; s2 * c3; c2]]%R.
Theorem B313_spec : B313 = Rz θ1 * Rx θ2 * Rz θ3.
Proof. meq; ring. Qed.
12. Body-two, 323
Definition B323 : mat 3 3 :=
l2m
[[c1 * c2 * c3 - s3 * s1; - c1 * c2 * s3 - c3 * s1; c1 * s2];
[s1 * c2 * c3 + s3 * c1; - s1 * c2 * s3 + c3 * c1; s1 * s2];
[- s2 * c3; s2 * s3; c2]]%R.
Theorem B323_spec : B323 = Rz θ1 * Ry θ2 * Rz θ3.
Proof. meq; ring. Qed.
l2m
[[c1 * c2 * c3 - s3 * s1; - c1 * c2 * s3 - c3 * s1; c1 * s2];
[s1 * c2 * c3 + s3 * c1; - s1 * c2 * s3 + c3 * c1; s1 * s2];
[- s2 * c3; s2 * s3; c2]]%R.
Theorem B323_spec : B323 = Rz θ1 * Ry θ2 * Rz θ3.
Proof. meq; ring. Qed.
13. Space-three, 123
Definition S123 : mat 3 3 :=
l2m
[[c2 * c3; s1 * s2 * c3 - s3 * c1; c1 * s2 * c3 + s3 * s1];
[c2 * s3; s1 * s2 * s3 + c3 * c1; c1 * s2 * s3 - c3 * s1];
[- s2; s1 * c2; c1 * c2]]%R.
Theorem S123_spec : S123 = Rz θ3 * Ry θ2 * Rx θ1.
Proof. meq; ring. Qed.
l2m
[[c2 * c3; s1 * s2 * c3 - s3 * c1; c1 * s2 * c3 + s3 * s1];
[c2 * s3; s1 * s2 * s3 + c3 * c1; c1 * s2 * s3 - c3 * s1];
[- s2; s1 * c2; c1 * c2]]%R.
Theorem S123_spec : S123 = Rz θ3 * Ry θ2 * Rx θ1.
Proof. meq; ring. Qed.
14. Space-three, 231
Definition S231 : mat 3 3 :=
l2m
[[c1 * c2; - s2; s1 * c2];
[c1 * s2 * c3 + s3 * s1; c2 * c3; s1 * s2 * c3 - s3 * c1];
[c1 * s2 * s3 - c3 * s1; c2 * s3; s1 * s2 * s3 + c3 * c1]]%R.
Theorem S231_spec : S231 = Rx θ3 * Rz θ2 * Ry θ1.
Proof. meq; ring. Qed.
l2m
[[c1 * c2; - s2; s1 * c2];
[c1 * s2 * c3 + s3 * s1; c2 * c3; s1 * s2 * c3 - s3 * c1];
[c1 * s2 * s3 - c3 * s1; c2 * s3; s1 * s2 * s3 + c3 * c1]]%R.
Theorem S231_spec : S231 = Rx θ3 * Rz θ2 * Ry θ1.
Proof. meq; ring. Qed.
15. Space-three, 312
Definition S312 : mat 3 3 :=
l2m
[[s1 * s2 * s3 + c3 * c1; c1 * s2 * s3 - c3 * s1; c2 * s3];
[s1 * c2; c1 * c2; - s2];
[s1 * s2 * c3 - s3 * c1; c1 * s2 * c3 + s3 * s1; c2 * c3]]%R.
Theorem S312_spec : S312 = Ry θ3 * Rx θ2 * Rz θ1.
Proof. meq; ring. Qed.
l2m
[[s1 * s2 * s3 + c3 * c1; c1 * s2 * s3 - c3 * s1; c2 * s3];
[s1 * c2; c1 * c2; - s2];
[s1 * s2 * c3 - s3 * c1; c1 * s2 * c3 + s3 * s1; c2 * c3]]%R.
Theorem S312_spec : S312 = Ry θ3 * Rx θ2 * Rz θ1.
Proof. meq; ring. Qed.
16. Space-three, 132
Definition S132 : mat 3 3 :=
l2m
[[c2 * c3; - c1 * s2 * c3 + s3 * s1; s1 * s2 * c3 + s3 * c1];
[s2; c1 * c2; - s1 * c2];
[- c2 * s3; c1 * s2 * s3 + c3 * s1; - s1 * s2 * s3 + c3 * c1]]%R.
Theorem S132_spec : S132 = Ry θ3 * Rz θ2 * Rx θ1.
Proof. meq; ring. Qed.
l2m
[[c2 * c3; - c1 * s2 * c3 + s3 * s1; s1 * s2 * c3 + s3 * c1];
[s2; c1 * c2; - s1 * c2];
[- c2 * s3; c1 * s2 * s3 + c3 * s1; - s1 * s2 * s3 + c3 * c1]]%R.
Theorem S132_spec : S132 = Ry θ3 * Rz θ2 * Rx θ1.
Proof. meq; ring. Qed.
17. Space-three, 213
Definition S213 : mat 3 3 :=
l2m
[[- s1 * s2 * s3 + c3 * c1; - c2 * s3; c1 * s2 * s3 + c3 * s1];
[s1 * s2 * c3 + s3 * c1; c2 * c3; - c1 * s2 * c3 + s3 * s1];
[- s1 * c2; s2; c1 * c2]]%R.
Theorem S213_spec : S213 = Rz θ3 * Rx θ2 * Ry θ1.
Proof. meq; ring. Qed.
l2m
[[- s1 * s2 * s3 + c3 * c1; - c2 * s3; c1 * s2 * s3 + c3 * s1];
[s1 * s2 * c3 + s3 * c1; c2 * c3; - c1 * s2 * c3 + s3 * s1];
[- s1 * c2; s2; c1 * c2]]%R.
Theorem S213_spec : S213 = Rz θ3 * Rx θ2 * Ry θ1.
Proof. meq; ring. Qed.
18. Space-three, 321
Definition S321 : mat 3 3 :=
l2m
[[c1 * c2; - s1 * c2; s2];
[c1 * s2 * s3 + c3 * s1; - s1 * s2 * s3 + c3 * c1; - c2 * s3];
[- c1 * s2 * c3 + s3 * s1; s1 * s2 * c3 + s3 * c1; c2 * c3]]%R.
Theorem S321_spec : S321 = Rx θ3 * Ry θ2 * Rz θ1.
Proof. meq; ring. Qed.
l2m
[[c1 * c2; - s1 * c2; s2];
[c1 * s2 * s3 + c3 * s1; - s1 * s2 * s3 + c3 * c1; - c2 * s3];
[- c1 * s2 * c3 + s3 * s1; s1 * s2 * c3 + s3 * c1; c2 * c3]]%R.
Theorem S321_spec : S321 = Rx θ3 * Ry θ2 * Rz θ1.
Proof. meq; ring. Qed.
19. Space-two, 121
Definition S121 : mat 3 3 :=
l2m
[[c2; s1 * s2; c1 * s2];
[s2 * s3; - s1 * c2 * s3 + c3 * c1; - c1 * c2 * s3 - c3 * s1];
[- s2 * c3; s1 * c2 * c3 + s3 * c1; c1 * c2 * c3 - s3 * s1]]%R.
Theorem S121_spec : S121 = Rx θ3 * Ry θ2 * Rx θ1.
Proof. meq; ring. Qed.
l2m
[[c2; s1 * s2; c1 * s2];
[s2 * s3; - s1 * c2 * s3 + c3 * c1; - c1 * c2 * s3 - c3 * s1];
[- s2 * c3; s1 * c2 * c3 + s3 * c1; c1 * c2 * c3 - s3 * s1]]%R.
Theorem S121_spec : S121 = Rx θ3 * Ry θ2 * Rx θ1.
Proof. meq; ring. Qed.
20. Space-two, 131
Definition S131 : mat 3 3 :=
l2m
[[c2; - c1 * s2; s1 * s2];
[s2 * c3; c1 * c2 * c3 - s3 * s1; - s1 * c2 * c3 - s3 * c1];
[s2 * s3; c1 * c2 * s3 + c3 * s1; - s1 * c2 * s3 + c3 * c1]]%R.
Theorem S131_spec : S131 = Rx θ3 * Rz θ2 * Rx θ1.
Proof. meq; ring. Qed.
l2m
[[c2; - c1 * s2; s1 * s2];
[s2 * c3; c1 * c2 * c3 - s3 * s1; - s1 * c2 * c3 - s3 * c1];
[s2 * s3; c1 * c2 * s3 + c3 * s1; - s1 * c2 * s3 + c3 * c1]]%R.
Theorem S131_spec : S131 = Rx θ3 * Rz θ2 * Rx θ1.
Proof. meq; ring. Qed.
21. Space-two, 212
Definition S212 : mat 3 3 :=
l2m
[[- s1 * c2 * s3 + c3 * c1; s2 * s3; c1 * c2 * s3 + c3 * s1];
[s1 * s2; c2; - c1 * s2];
[-s1 * c2 * c3 - s3 * c1; s2 * c3; c1 * c2 * c3 - s3 * s1]]%R.
Theorem S212_spec : S212 = Ry θ3 * Rx θ2 * Ry θ1.
Proof. meq; ring. Qed.
l2m
[[- s1 * c2 * s3 + c3 * c1; s2 * s3; c1 * c2 * s3 + c3 * s1];
[s1 * s2; c2; - c1 * s2];
[-s1 * c2 * c3 - s3 * c1; s2 * c3; c1 * c2 * c3 - s3 * s1]]%R.
Theorem S212_spec : S212 = Ry θ3 * Rx θ2 * Ry θ1.
Proof. meq; ring. Qed.
22. Space-two, 232
Definition S232 : mat 3 3 :=
l2m
[[c1 * c2 * c3 - s3 * s1; - s2 * c3; s1 * c2 * c3 + s3 * c1];
[c1 * s2; c2; s1 * s2];
[- c1 * c2 * s3 - c3 * s1; s2 * s3; - s1 * c2 * s3 + c3 * c1]]%R.
Theorem S232_spec : S232 = Ry θ3 * Rz θ2 * Ry θ1.
Proof. meq; ring. Qed.
l2m
[[c1 * c2 * c3 - s3 * s1; - s2 * c3; s1 * c2 * c3 + s3 * c1];
[c1 * s2; c2; s1 * s2];
[- c1 * c2 * s3 - c3 * s1; s2 * s3; - s1 * c2 * s3 + c3 * c1]]%R.
Theorem S232_spec : S232 = Ry θ3 * Rz θ2 * Ry θ1.
Proof. meq; ring. Qed.
23. Space-two, 313
Definition S313 : mat 3 3 :=
l2m
[[- s1 * c2 * s3 + c3 * c1; - c1 * c2 * s3 - c3 * s1; s2 * s3];
[s1 * c2 * c3 + s3 * c1; c1 * c2 * c3 - s3 * s1; - s2 * c3];
[s1 * s2; c1 * s2; c2]]%R.
Theorem S313_spec : S313 = Rz θ3 * Rx θ2 * Rz θ1.
Proof. meq; ring. Qed.
l2m
[[- s1 * c2 * s3 + c3 * c1; - c1 * c2 * s3 - c3 * s1; s2 * s3];
[s1 * c2 * c3 + s3 * c1; c1 * c2 * c3 - s3 * s1; - s2 * c3];
[s1 * s2; c1 * s2; c2]]%R.
Theorem S313_spec : S313 = Rz θ3 * Rx θ2 * Rz θ1.
Proof. meq; ring. Qed.
24. Space-two, 323
Definition S323 : mat 3 3 :=
l2m
[[c1 * c2 * c3 - s3 * s1; - s1 * c2 * c3 - s3 * c1; s2 * c3];
[c1 * c2 * s3 + c3 * s1; - s1 * c2 * s3 + c3 * c1; s2 * s3];
[-c1 * s2; s1 * s2; c2]]%R.
Theorem S323_spec : S323 = Rz θ3 * Ry θ2 * Rz θ1.
Proof. meq; ring. Qed.
End EulerAngle24.
l2m
[[c1 * c2 * c3 - s3 * s1; - s1 * c2 * c3 - s3 * c1; s2 * c3];
[c1 * c2 * s3 + c3 * s1; - s1 * c2 * s3 + c3 * c1; s2 * s3];
[-c1 * s2; s1 * s2; c2]]%R.
Theorem S323_spec : S323 = Rz θ3 * Ry θ2 * Rz θ1.
Proof. meq; ring. Qed.
End EulerAngle24.
Because the relativity, the 24 ways are actually only 12 ways.
B123 = S321, B132 = S231, B213 = S312, B231 = S132, B312 = S213, B321 = S123,
B121 = S121, B131 = S131, B212 = S212, B232 = S232, B313 = S313, B323 = S323
Section EulerAngle24_only_half.
Variable a1 a2 a3 : R.
Lemma B123_eq_S321 : B123 a1 a2 a3 = S321 a3 a2 a1.
Proof. meq; ring. Qed.
Lemma B132_eq_S231 : B132 a1 a3 a2 = S231 a2 a3 a1.
Proof. meq; ring. Qed.
Lemma B213_eq_S312 : B213 a2 a1 a3 = S312 a3 a1 a2.
Proof. meq; ring. Qed.
Lemma B231_eq_S132 : B231 a2 a3 a1 = S132 a1 a3 a2.
Proof. meq; ring. Qed.
Lemma B312_eq_S213 : B312 a3 a1 a2 = S213 a2 a1 a3.
Proof. meq; ring. Qed.
Lemma B321_eq_S123 : B321 a3 a2 a1 = S123 a1 a2 a3.
Proof. meq; ring. Qed.
Lemma B121_eq_S121 : B121 a1 a2 a1 = S121 a1 a2 a1.
Proof. meq; ring. Qed.
Lemma B131_eq_S131 : B131 a1 a3 a1 = S131 a1 a3 a1.
Proof. meq; ring. Qed.
Lemma B212_eq_S212 : B212 a2 a1 a2 = S212 a2 a1 a2.
Proof. meq; ring. Qed.
Lemma B232_eq_S232 : B232 a2 a3 a2 = S232 a2 a3 a2.
Proof. meq; ring. Qed.
Lemma B313_eq_S313 : B313 a3 a1 a3 = S313 a3 a1 a3.
Proof. meq; ring. Qed.
Lemma B323_eq_S323 : B323 a3 a2 a3 = S323 a3 a2 a3.
Proof. meq; ring. Qed.
End EulerAngle24_only_half.
Variable a1 a2 a3 : R.
Lemma B123_eq_S321 : B123 a1 a2 a3 = S321 a3 a2 a1.
Proof. meq; ring. Qed.
Lemma B132_eq_S231 : B132 a1 a3 a2 = S231 a2 a3 a1.
Proof. meq; ring. Qed.
Lemma B213_eq_S312 : B213 a2 a1 a3 = S312 a3 a1 a2.
Proof. meq; ring. Qed.
Lemma B231_eq_S132 : B231 a2 a3 a1 = S132 a1 a3 a2.
Proof. meq; ring. Qed.
Lemma B312_eq_S213 : B312 a3 a1 a2 = S213 a2 a1 a3.
Proof. meq; ring. Qed.
Lemma B321_eq_S123 : B321 a3 a2 a1 = S123 a1 a2 a3.
Proof. meq; ring. Qed.
Lemma B121_eq_S121 : B121 a1 a2 a1 = S121 a1 a2 a1.
Proof. meq; ring. Qed.
Lemma B131_eq_S131 : B131 a1 a3 a1 = S131 a1 a3 a1.
Proof. meq; ring. Qed.
Lemma B212_eq_S212 : B212 a2 a1 a2 = S212 a2 a1 a2.
Proof. meq; ring. Qed.
Lemma B232_eq_S232 : B232 a2 a3 a2 = S232 a2 a3 a2.
Proof. meq; ring. Qed.
Lemma B313_eq_S313 : B313 a3 a1 a3 = S313 a3 a1 a3.
Proof. meq; ring. Qed.
Lemma B323_eq_S323 : B323 a3 a2 a3 = S323 a3 a2 a3.
Proof. meq; ring. Qed.
End EulerAngle24_only_half.
Convert Rotation Matrix to Euler angles by S123
奇异性问题的存在性
Claim: If θ = kπ+π/2, then we can not uniquely determine ϕ and ψ.
If θ = π/2, then the rotation matrix has following form.
Lemma S123_θ_eq_pi2 : forall (ϕ θ ψ : R),
θ = PI/2 ->
S123 ϕ θ ψ =
l2m [[0; - sin (ψ - ϕ); cos (ψ - ϕ)];
[0; cos (ψ - ϕ); sin (ψ - ϕ)];
[-1; 0; 0]].
Proof.
intros; rewrite H. pose proof cos_PI2. pose proof sin_PI2. cbv in H0, H1.
meq. all: try ra.
Qed.
θ = PI/2 ->
S123 ϕ θ ψ =
l2m [[0; - sin (ψ - ϕ); cos (ψ - ϕ)];
[0; cos (ψ - ϕ); sin (ψ - ϕ)];
[-1; 0; 0]].
Proof.
intros; rewrite H. pose proof cos_PI2. pose proof sin_PI2. cbv in H0, H1.
meq. all: try ra.
Qed.
If θ = -π/2, then the rotation matrix has following form.
Lemma S123_θ_eq_pi2_neg : forall (ϕ θ ψ : R),
θ = -PI/2 ->
S123 ϕ θ ψ =
l2m [[0; - sin (ψ + ϕ); - cos (ψ + ϕ)];
[0; cos (ψ + ϕ); - sin (ψ + ϕ)];
[1; 0; 0]].
Proof.
intros; rewrite H. pose proof cos_PI2. pose proof sin_PI2. cbv in H0, H1.
meq. all: try ra.
Qed.
θ = -PI/2 ->
S123 ϕ θ ψ =
l2m [[0; - sin (ψ + ϕ); - cos (ψ + ϕ)];
[0; cos (ψ + ϕ); - sin (ψ + ϕ)];
[1; 0; 0]].
Proof.
intros; rewrite H. pose proof cos_PI2. pose proof sin_PI2. cbv in H0, H1.
meq. all: try ra.
Qed.
If θ = π/2, then there are infinite ϕ can generate a same matrix.
Lemma S123_singularity_ϕ_when_θ_eq_pi2 : forall (ϕ θ ψ : R),
θ = PI/2 -> forall ϕ', (exists ψ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !S123_θ_eq_pi2; auto.
instantiate (1:=ψ - ϕ + ϕ'). repeat (f_equal; try lra).
Qed.
θ = PI/2 -> forall ϕ', (exists ψ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !S123_θ_eq_pi2; auto.
instantiate (1:=ψ - ϕ + ϕ'). repeat (f_equal; try lra).
Qed.
If θ = -π/2, then there are infinite ϕ can generate a same matrix.
Lemma S123_singularity_ϕ_when_θ_eq_pi2_neg : forall (ϕ θ ψ : R),
θ = -PI/2 -> forall ϕ', (exists ψ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !S123_θ_eq_pi2_neg; auto.
instantiate (1:=ψ + ϕ - ϕ'). repeat (f_equal; try lra).
Qed.
θ = -PI/2 -> forall ϕ', (exists ψ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !S123_θ_eq_pi2_neg; auto.
instantiate (1:=ψ + ϕ - ϕ'). repeat (f_equal; try lra).
Qed.
If θ = ±π/2, then there are infinite ϕ can generate a same matrix.
Theorem S123_singularity_ϕ : forall (ϕ θ ψ : R),
(θ = PI/2 \/ θ = -PI/2) -> forall ϕ', (exists ψ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. destruct H.
apply S123_singularity_ϕ_when_θ_eq_pi2; auto.
apply S123_singularity_ϕ_when_θ_eq_pi2_neg; auto.
Qed.
(θ = PI/2 \/ θ = -PI/2) -> forall ϕ', (exists ψ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. destruct H.
apply S123_singularity_ϕ_when_θ_eq_pi2; auto.
apply S123_singularity_ϕ_when_θ_eq_pi2_neg; auto.
Qed.
If θ = π/2, then there are infinite ψ can generate a same matrix.
Lemma S123_singularity_ψ_when_θ_eq_pi2 : forall (ϕ θ ψ : R),
θ = PI/2 -> forall ψ', (exists ϕ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !S123_θ_eq_pi2; auto.
instantiate (1:=ψ' - ψ + ϕ). repeat (f_equal; try lra).
Qed.
θ = PI/2 -> forall ψ', (exists ϕ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !S123_θ_eq_pi2; auto.
instantiate (1:=ψ' - ψ + ϕ). repeat (f_equal; try lra).
Qed.
If θ = -π/2, then there are infinite ψ can generate a same matrix.
Lemma S123_singularity_ψ_when_θ_eq_pi2_neg : forall (ϕ θ ψ : R),
θ = -PI/2 -> forall ψ', (exists ϕ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !S123_θ_eq_pi2_neg; auto.
instantiate (1:=ψ + ϕ - ψ'). repeat (f_equal; try lra).
Qed.
θ = -PI/2 -> forall ψ', (exists ϕ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !S123_θ_eq_pi2_neg; auto.
instantiate (1:=ψ + ϕ - ψ'). repeat (f_equal; try lra).
Qed.
If θ = ±π/2, then there are infinite ψ can generate a same matrix.
Theorem S123_singularity_ψ : forall (ϕ θ ψ : R),
(θ = PI/2 \/ θ = -PI/2) -> forall ψ', (exists ϕ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. destruct H.
apply S123_singularity_ψ_when_θ_eq_pi2; auto.
apply S123_singularity_ψ_when_θ_eq_pi2_neg; auto.
Qed.
End singularity.
Module alg1.
Definition ϕ' (C : smat 3) := atan (C.32 / C.33).
Definition θ' (C : smat 3) := asin (-C.31).
Definition ψ' (C : smat 3) := atan (C.21 / C.11).
Theorem alg_spec : forall (ϕ θ ψ : R) (C : smat 3),
-PI/2 < ϕ < PI/2 ->
-PI/2 < θ < PI/2 ->
-PI/2 < ψ < PI/2 ->
C = S123 ϕ θ ψ ->
ϕ' C = ϕ /\ θ' C = θ /\ ψ' C = ψ.
Proof.
intros. v2eALL C.
apply Vector.v3eq_iff in H2. destruct H2,H3.
apply Vector.v3eq_iff in H2,H3,H4. cbv in H2,H3,H4.
cbv. logic.
- rewrite H5,H6. ra. rewrite atan_tan; auto.
- rewrite H4. ra. rewrite asin_sin; ra.
- rewrite H3,H2. ra. rewrite atan_tan; ra.
Qed.
End alg1.
Module alg2.
Definition ϕ' (C : smat 3) := atan2 (C.32) (C.33).
Definition θ' (C : smat 3) := asin (- C.31).
Definition ψ' (C : smat 3) := atan2 (C.21) (C.11).
Theorem alg_spec : forall (ϕ θ ψ : R) (C : smat 3),
-PI < ϕ < PI ->
-PI/2 < θ < PI/2 ->
-PI < ψ < PI ->
C = S123 ϕ θ ψ ->
ϕ' C = ϕ /\ θ' C = θ /\ ψ' C = ψ.
Proof.
intros. cbv. rewrite !H2; auto. cbv; ra.
assert (0 < cos θ). { apply cos_gt_0; try lra. }
repeat split.
- rewrite atan2_sin_cos_eq1; auto. lra.
- rewrite asin_sin; ra.
- rewrite !(Rmult_comm (cos θ)). rewrite atan2_sin_cos_eq1; auto. lra.
Qed.
End alg2.
Module alg3.
(θ = PI/2 \/ θ = -PI/2) -> forall ψ', (exists ϕ', S123 ϕ' θ ψ' = S123 ϕ θ ψ).
Proof.
intros. destruct H.
apply S123_singularity_ψ_when_θ_eq_pi2; auto.
apply S123_singularity_ψ_when_θ_eq_pi2_neg; auto.
Qed.
End singularity.
Module alg1.
Definition ϕ' (C : smat 3) := atan (C.32 / C.33).
Definition θ' (C : smat 3) := asin (-C.31).
Definition ψ' (C : smat 3) := atan (C.21 / C.11).
Theorem alg_spec : forall (ϕ θ ψ : R) (C : smat 3),
-PI/2 < ϕ < PI/2 ->
-PI/2 < θ < PI/2 ->
-PI/2 < ψ < PI/2 ->
C = S123 ϕ θ ψ ->
ϕ' C = ϕ /\ θ' C = θ /\ ψ' C = ψ.
Proof.
intros. v2eALL C.
apply Vector.v3eq_iff in H2. destruct H2,H3.
apply Vector.v3eq_iff in H2,H3,H4. cbv in H2,H3,H4.
cbv. logic.
- rewrite H5,H6. ra. rewrite atan_tan; auto.
- rewrite H4. ra. rewrite asin_sin; ra.
- rewrite H3,H2. ra. rewrite atan_tan; ra.
Qed.
End alg1.
Module alg2.
Definition ϕ' (C : smat 3) := atan2 (C.32) (C.33).
Definition θ' (C : smat 3) := asin (- C.31).
Definition ψ' (C : smat 3) := atan2 (C.21) (C.11).
Theorem alg_spec : forall (ϕ θ ψ : R) (C : smat 3),
-PI < ϕ < PI ->
-PI/2 < θ < PI/2 ->
-PI < ψ < PI ->
C = S123 ϕ θ ψ ->
ϕ' C = ϕ /\ θ' C = θ /\ ψ' C = ψ.
Proof.
intros. cbv. rewrite !H2; auto. cbv; ra.
assert (0 < cos θ). { apply cos_gt_0; try lra. }
repeat split.
- rewrite atan2_sin_cos_eq1; auto. lra.
- rewrite asin_sin; ra.
- rewrite !(Rmult_comm (cos θ)). rewrite atan2_sin_cos_eq1; auto. lra.
Qed.
End alg2.
Module alg3.
sign of a real number
Definition Rsign (r : R) : R := if r <? 0 then -1 else 1.
Section sec.
Variable C : smat 3.
Definition case1_cond : bool := (C.11 =? 0) && (C.21 =? 0).
Definition case1_values : vec 3 :=
l2v [0; (Rsign (-C.31)) * (PI / 2); atan2 (-C.12) (C.22)].
Definition case2_params : mat 3 2 :=
let θ_0 := asin (-C.31) in
l2m [[atan2 (C.32) (C.33); atan2 (-C.32) (-C.33)];
[θ_0; Rsign θ_0 * PI - θ_0];
[atan2 (C.21) (C.11); atan2 (-C.21) (-C.11)]].
Definition find_best : (R*R*R*R) :=
let gen_val (ϕ θ ψ : R) : R*R*R*R := (ϕ, θ, ψ, mnormF (C - S123 ϕ θ ψ)%M) in
let m := case2_params in
let a111 := gen_val (m.11) (m.21) (m.31) in
let a112 := gen_val (m.11) (m.21) (m.32) in
let a121 := gen_val (m.11) (m.22) (m.31) in
let a122 := gen_val (m.11) (m.22) (m.32) in
let a211 := gen_val (m.12) (m.21) (m.31) in
let a212 := gen_val (m.12) (m.21) (m.32) in
let a221 := gen_val (m.12) (m.22) (m.31) in
let a222 := gen_val (m.12) (m.22) (m.32) in
let l := [a111;a112;a121;a122;a211;a212;a221;a222] in
list_min a111
(fun x y => match x, y with (_,_,_,x1),(_,_,_,y1) => x1 <? y1 end)
l.
Definition case2_values : vec 3 :=
let '(ϕ,θ,ψ,_) := find_best in
l2v [ϕ; θ; ψ].
Section sec.
Variable C : smat 3.
Definition case1_cond : bool := (C.11 =? 0) && (C.21 =? 0).
Definition case1_values : vec 3 :=
l2v [0; (Rsign (-C.31)) * (PI / 2); atan2 (-C.12) (C.22)].
Definition case2_params : mat 3 2 :=
let θ_0 := asin (-C.31) in
l2m [[atan2 (C.32) (C.33); atan2 (-C.32) (-C.33)];
[θ_0; Rsign θ_0 * PI - θ_0];
[atan2 (C.21) (C.11); atan2 (-C.21) (-C.11)]].
Definition find_best : (R*R*R*R) :=
let gen_val (ϕ θ ψ : R) : R*R*R*R := (ϕ, θ, ψ, mnormF (C - S123 ϕ θ ψ)%M) in
let m := case2_params in
let a111 := gen_val (m.11) (m.21) (m.31) in
let a112 := gen_val (m.11) (m.21) (m.32) in
let a121 := gen_val (m.11) (m.22) (m.31) in
let a122 := gen_val (m.11) (m.22) (m.32) in
let a211 := gen_val (m.12) (m.21) (m.31) in
let a212 := gen_val (m.12) (m.21) (m.32) in
let a221 := gen_val (m.12) (m.22) (m.31) in
let a222 := gen_val (m.12) (m.22) (m.32) in
let l := [a111;a112;a121;a122;a211;a212;a221;a222] in
list_min a111
(fun x y => match x, y with (_,_,_,x1),(_,_,_,y1) => x1 <? y1 end)
l.
Definition case2_values : vec 3 :=
let '(ϕ,θ,ψ,_) := find_best in
l2v [ϕ; θ; ψ].
If the matrix is identity matrix, there are two possible solutions
Definition case3_cond : bool :=
(C.11 =? 1) && (C.33 =? 1) && (C.21 =? 0) && (C.32 =? 0) && (C.31 =? 0).
(C.11 =? 1) && (C.33 =? 1) && (C.21 =? 0) && (C.32 =? 0) && (C.31 =? 0).
If the euler angles is {0,0,0} or {π,π,π}, then the matrix is identity matrix
Lemma case3_opts_1_eq_mat1 :
S123 0 0 0 = mat1.
Proof. meq; ra. Qed.
Lemma case3_opts_2_eq_mat1 :
S123 PI PI PI = mat1.
Proof. meq; ra. Qed.
Definition case3_values (old : vec 3) : vec 3 :=
let closest (old opt1 opt2 : R) : R :=
if Rabs (old - opt1) <? Rabs (old - opt2) then opt1 else opt2 in
l2v [
closest (old.1) 0 PI;
closest (old.2) 0 PI;
closest (old.3) 0 PI
].
S123 0 0 0 = mat1.
Proof. meq; ra. Qed.
Lemma case3_opts_2_eq_mat1 :
S123 PI PI PI = mat1.
Proof. meq; ra. Qed.
Definition case3_values (old : vec 3) : vec 3 :=
let closest (old opt1 opt2 : R) : R :=
if Rabs (old - opt1) <? Rabs (old - opt2) then opt1 else opt2 in
l2v [
closest (old.1) 0 PI;
closest (old.2) 0 PI;
closest (old.3) 0 PI
].
final algorithm
Definition euler_angles (old : vec 3) : vec 3 :=
if case1_cond
then case1_values
else if case3_cond
then case3_values old
else case2_values.
if case1_cond
then case1_values
else if case3_cond
then case3_values old
else case2_values.
This algorithm havn't been verified yet.
2. Body-three, 123
奇异性问题的存在性
Claim: If θ = kπ+π/2, then we can not uniquely determine ϕ and ψ.
If θ = π/2, then the rotation matrix has following form.
Lemma B123_θ_eq_pi2 : forall (ϕ θ ψ : R),
θ = PI/2 ->
B123 ϕ θ ψ =
l2m [[0; 0; 1];
[sin (ϕ + ψ); cos (ϕ + ψ); 0];
[- cos (ϕ + ψ); sin (ϕ + ψ); 0]].
Proof.
intros; rewrite H.
pose proof cos_PI2. pose proof sin_PI2. cbv in H0, H1. meq; ra.
Qed.
θ = PI/2 ->
B123 ϕ θ ψ =
l2m [[0; 0; 1];
[sin (ϕ + ψ); cos (ϕ + ψ); 0];
[- cos (ϕ + ψ); sin (ϕ + ψ); 0]].
Proof.
intros; rewrite H.
pose proof cos_PI2. pose proof sin_PI2. cbv in H0, H1. meq; ra.
Qed.
If θ = -π/2, then the rotation matrix has following form.
Lemma B123_θ_eq_pi2_neg : forall (ϕ θ ψ : R),
θ = -PI/2 ->
B123 ϕ θ ψ =
l2m [[0; 0; -1];
[sin (ψ - ϕ); cos (ψ - ϕ); 0];
[cos (ψ - ϕ); - sin (ψ - ϕ); 0]].
Proof.
intros; rewrite H.
pose proof cos_PI2. pose proof sin_PI2. cbv in H0, H1. meq; ra.
Qed.
θ = -PI/2 ->
B123 ϕ θ ψ =
l2m [[0; 0; -1];
[sin (ψ - ϕ); cos (ψ - ϕ); 0];
[cos (ψ - ϕ); - sin (ψ - ϕ); 0]].
Proof.
intros; rewrite H.
pose proof cos_PI2. pose proof sin_PI2. cbv in H0, H1. meq; ra.
Qed.
If θ = π/2, then there are infinite ϕ can generate a same matrix.
Lemma B123_singularity_ϕ_when_θ_eq_pi2 : forall (ϕ θ ψ : R),
θ = PI/2 -> forall ϕ', (exists ψ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !B123_θ_eq_pi2; auto.
instantiate (1:=ψ + ϕ - ϕ'). repeat (f_equal; try lra).
Qed.
θ = PI/2 -> forall ϕ', (exists ψ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !B123_θ_eq_pi2; auto.
instantiate (1:=ψ + ϕ - ϕ'). repeat (f_equal; try lra).
Qed.
If θ = -π/2, then there are infinite ϕ can generate a same matrix.
Lemma B123_singularity_ϕ_when_θ_eq_pi2_neg : forall (ϕ θ ψ : R),
θ = -PI/2 -> forall ϕ', (exists ψ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !B123_θ_eq_pi2_neg; auto.
instantiate (1:=ψ - ϕ + ϕ'). repeat (f_equal; try lra).
Qed.
θ = -PI/2 -> forall ϕ', (exists ψ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !B123_θ_eq_pi2_neg; auto.
instantiate (1:=ψ - ϕ + ϕ'). repeat (f_equal; try lra).
Qed.
If θ = ±π/2, then there are infinite ϕ can generate a same matrix.
Theorem B123_singularity_ϕ : forall (ϕ θ ψ : R),
(θ = PI/2 \/ θ = -PI/2) -> forall ϕ', (exists ψ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. destruct H.
apply B123_singularity_ϕ_when_θ_eq_pi2; auto.
apply B123_singularity_ϕ_when_θ_eq_pi2_neg; auto.
Qed.
(θ = PI/2 \/ θ = -PI/2) -> forall ϕ', (exists ψ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. destruct H.
apply B123_singularity_ϕ_when_θ_eq_pi2; auto.
apply B123_singularity_ϕ_when_θ_eq_pi2_neg; auto.
Qed.
If θ = π/2, then there are infinite ψ can generate a same matrix.
Lemma B123_singularity_ψ_when_θ_eq_pi2 : forall (ϕ θ ψ : R),
θ = PI/2 -> forall ψ', (exists ϕ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !B123_θ_eq_pi2; auto.
instantiate (1:=ψ + ϕ - ψ'). repeat (f_equal; try lra).
Qed.
θ = PI/2 -> forall ψ', (exists ϕ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !B123_θ_eq_pi2; auto.
instantiate (1:=ψ + ϕ - ψ'). repeat (f_equal; try lra).
Qed.
If θ = -π/2, then there are infinite ψ can generate a same matrix.
Lemma B123_singularity_ψ_when_θ_eq_pi2_neg : forall (ϕ θ ψ : R),
θ = -PI/2 -> forall ψ', (exists ϕ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !B123_θ_eq_pi2_neg; auto.
instantiate (1:=-ψ + ϕ + ψ'). repeat (f_equal; try lra).
Qed.
θ = -PI/2 -> forall ψ', (exists ϕ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. eexists. rewrite !B123_θ_eq_pi2_neg; auto.
instantiate (1:=-ψ + ϕ + ψ'). repeat (f_equal; try lra).
Qed.
If θ = ±π/2, then there are infinite ψ can generate a same matrix.
Theorem B123_singularity_ψ : forall (ϕ θ ψ : R),
(θ = PI/2 \/ θ = -PI/2) -> forall ψ', (exists ϕ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. destruct H.
apply B123_singularity_ψ_when_θ_eq_pi2; auto.
apply B123_singularity_ψ_when_θ_eq_pi2_neg; auto.
Qed.
End singularity.
Module alg1.
Definition ϕ' (C : smat 3) := atan (- C.23 / C.33).
Definition θ' (C : smat 3) := asin (C.13).
Definition ψ' (C : smat 3) := atan (- C.12 / C.11).
Theorem alg_spec : forall (ϕ θ ψ : R) (C : smat 3),
-PI/2 < ϕ < PI/2 ->
-PI/2 < θ < PI/2 ->
-PI/2 < ψ < PI/2 ->
C = B123 ϕ θ ψ ->
ϕ' C = ϕ /\ θ' C = θ /\ ψ' C = ψ.
Proof.
intros. v2eALL C.
apply Vector.v3eq_iff in H2. destruct H2,H3.
apply Vector.v3eq_iff in H2,H3,H4. cbv in H2,H3,H4.
cbv. logic.
- rewrite H8,H6. ra. rewrite atan_tan; auto.
- rewrite H10. rewrite asin_sin; ra.
- rewrite H9,H2. ra. rewrite atan_tan; ra.
Qed.
End alg1.
Module alg2.
Definition ϕ' (C : smat 3) := atan2 (- C.23) (C.33).
Definition θ' (C : smat 3) := asin (C.13).
Definition ψ' (C : smat 3) := atan2 (- C.12) (C.11).
Theorem alg_spec : forall (ϕ θ ψ : R) (C : smat 3),
-PI < ϕ < PI ->
-PI/2 < θ < PI/2 ->
-PI < ψ < PI ->
C = B123 ϕ θ ψ ->
ϕ' C = ϕ /\ θ' C = θ /\ ψ' C = ψ.
Proof.
intros. cbv. rewrite !H2; auto. cbv; ra.
assert (0 < cos θ). { apply cos_gt_0; try lra. }
repeat split.
- rewrite atan2_sin_cos_eq1; auto. lra.
- rewrite asin_sin; ra.
- rewrite !(Rmult_comm (cos θ)). rewrite atan2_sin_cos_eq1; auto. lra.
Qed.
End alg2.
End R2Euler_B123.
(θ = PI/2 \/ θ = -PI/2) -> forall ψ', (exists ϕ', B123 ϕ' θ ψ' = B123 ϕ θ ψ).
Proof.
intros. destruct H.
apply B123_singularity_ψ_when_θ_eq_pi2; auto.
apply B123_singularity_ψ_when_θ_eq_pi2_neg; auto.
Qed.
End singularity.
Module alg1.
Definition ϕ' (C : smat 3) := atan (- C.23 / C.33).
Definition θ' (C : smat 3) := asin (C.13).
Definition ψ' (C : smat 3) := atan (- C.12 / C.11).
Theorem alg_spec : forall (ϕ θ ψ : R) (C : smat 3),
-PI/2 < ϕ < PI/2 ->
-PI/2 < θ < PI/2 ->
-PI/2 < ψ < PI/2 ->
C = B123 ϕ θ ψ ->
ϕ' C = ϕ /\ θ' C = θ /\ ψ' C = ψ.
Proof.
intros. v2eALL C.
apply Vector.v3eq_iff in H2. destruct H2,H3.
apply Vector.v3eq_iff in H2,H3,H4. cbv in H2,H3,H4.
cbv. logic.
- rewrite H8,H6. ra. rewrite atan_tan; auto.
- rewrite H10. rewrite asin_sin; ra.
- rewrite H9,H2. ra. rewrite atan_tan; ra.
Qed.
End alg1.
Module alg2.
Definition ϕ' (C : smat 3) := atan2 (- C.23) (C.33).
Definition θ' (C : smat 3) := asin (C.13).
Definition ψ' (C : smat 3) := atan2 (- C.12) (C.11).
Theorem alg_spec : forall (ϕ θ ψ : R) (C : smat 3),
-PI < ϕ < PI ->
-PI/2 < θ < PI/2 ->
-PI < ψ < PI ->
C = B123 ϕ θ ψ ->
ϕ' C = ϕ /\ θ' C = θ /\ ψ' C = ψ.
Proof.
intros. cbv. rewrite !H2; auto. cbv; ra.
assert (0 < cos θ). { apply cos_gt_0; try lra. }
repeat split.
- rewrite atan2_sin_cos_eq1; auto. lra.
- rewrite asin_sin; ra.
- rewrite !(Rmult_comm (cos θ)). rewrite atan2_sin_cos_eq1; auto. lra.
Qed.
End alg2.
End R2Euler_B123.