-
Notifications
You must be signed in to change notification settings - Fork 2
/
rot.v
2400 lines (2184 loc) · 95.5 KB
/
rot.v
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
(* coq-robot (c) 2017 AIST and INRIA. License: LGPL-2.1-or-later. *)
From HB Require Import structures.
Require Import NsatzTactic.
From mathcomp Require Import all_ssreflect ssralg ssrint ssrnum rat poly.
From mathcomp Require Import closed_field polyrcf matrix mxalgebra mxpoly zmodp.
From mathcomp Require Import realalg reals complex.
From mathcomp Require Import interval trigo fingroup perm.
Require Import extra_trigo.
Require Import ssr_ext euclidean skew vec_angle frame.
From mathcomp.analysis Require Import forms.
(******************************************************************************)
(* Rotations *)
(* *)
(* This file develops the theory of 3D rotations with results such as *)
(* Rodrigues formula, the fact that any rotation matrix can be represented *)
(* by its exponential coordinates, angle-axis representation, Euler angles, *)
(* etc. See also quaternion.v for rotations using quaternions. *)
(* *)
(* RO a, RO' a == two dimensional rotations of angle a *)
(* *)
(* Elementary rotations (row vector convention): *)
(* Rx a, Rx' a == rotations about axis x of angle a *)
(* Ry a == rotation about axis y of angle a *)
(* Rz a == rotation about axis z of angle a *)
(* *)
(* isRot a u f == f is a rotation of angle a w.r.t. vector u *)
(* sample lemmas: *)
(* all rotations around a vector of angle a have trace "1 + 2 * cos a" *)
(* equivalence SO[R]_3 <-> Rot (isRot_SO, SO_is_Rot) *)
(* *)
(* `e(a, M) == specialized exponential map for angle a and matrix M *)
(* `e^(a, w) == specialized exponential map for the matrix \S(w), i.e., the *)
(* skew-symmetric matrix corresponding to vector w *)
(* sample lemmas: *)
(* inverse of the exponential map, *)
(* exponential map of a skew matrix is a rotation *)
(* *)
(* rodrigues u a w == linear combination of the vectors u, (u *d w)w, w *v u *)
(* that provides an alternative expression for the vector *)
(* u * e^(a,w) *)
(* *)
(* Angle-axis representation: *)
(* Aa.angle M == angle of angle-axis representation for the matrix M *)
(* Aa.vaxis M == axis of angle-axis representation for the matrix M *)
(* sample lemma *)
(* a rotation matrix has Aa.angle M and normalize (Aa.vaxis M) for *)
(* exponential coordinates *)
(* *)
(* Composition of elementary rotations (row vector convention): *)
(* Rzyz a b c == composition of a Rz rotation of angle c, a Ry rotation of *)
(* angle b, and a Rz rotation of angle a *)
(* Rxyz a b c == composition of a Rx rotation of angle c, a Ry rotation of *)
(* angle b, and a Rz notation of angle a *)
(* *)
(* ZYZ angles given a rotation matrix M (ref: [sciavicco] 2.4.1): *)
(* with zyz_b in ]0;pi[: *)
(* zyz_a M == angle of the last Rz rotation *)
(* zyz_b M == angle of the Ry rotation *)
(* zyz_c M == angle of the first Rz rotation *)
(* *)
(* Roll-Pitch-Yaw (ZYX) angles given a rotation matrix M *)
(* with pitch in ]-pi/2;pi/2[ (ref: [sciavicco] 2.4.2): *)
(* rpy_a M == angle about axis z (roll) *)
(* rpy_b M == angle about axis y (pitch) *)
(* rpy_c M == angle about axis x (yaw) *)
(* *)
(* Alternative formulation of ZYX angles: *)
(* (ref: [Gregory G. Slabaugh, Computer Euler angles from a rotation matrix]) *)
(* euler_a == angle about z *)
(* euler_b == angle about y *)
(* euler_c == angle about x *)
(******************************************************************************)
Reserved Notation "'`e(' a ',' M ')'" (format "'`e(' a ',' M ')'").
Reserved Notation "'`e^(' a ',' w ')'" (format "'`e^(' a ',' w ')'").
Set Implicit Arguments.
Unset Strict Implicit.
Unset Printing Implicit Defensive.
(* TODO: overrides forms.v *)
Notation "u '``_' i" := (u (@GRing.zero _) i) : ring_scope.
Import Order.TTheory GRing.Theory Num.Def Num.Theory.
Local Open Scope ring_scope.
Section two_dimensional_rotation.
Variable T : realType.
Implicit Types (a b : T) (M : 'M[T]_2).
Definition RO a := col_mx2 (row2 (cos a) (sin a)) (row2 (- sin a) (cos a)).
Lemma trmx_RO a : (RO a)^T = RO (- a).
Proof.
apply/matrixP => i j.
rewrite !mxE /= cosN sinN opprK.
case: ifPn => [/eqP ->{j}|].
case: ifPn => [/eqP ->{i}|]; first by rewrite !mxE.
by rewrite ifnot01 => /eqP ->{i}; rewrite eqxx !mxE.
rewrite ifnot01 => /eqP ->; rewrite eqxx.
case: ifPn => [/eqP ->{i}|]; rewrite ?mxE //= ifnot01 => /eqP ->{i}.
by rewrite eqxx /= mxE.
Qed.
Lemma tr_RO a : \tr (RO a) = (cos a) *+ 2.
Proof. by rewrite /mxtrace sum2E !mxE /= mulr2n. Qed.
Lemma RO_is_O a : RO a \is 'O[T]_2.
Proof.
apply/orthogonal2P; rewrite !rowK /= !dotmulE !sum2E !mxE /= -!expr2 cos2Dsin2.
by rewrite addrC mulrN mulrC subrr addrC mulNr mulrC subrr sqrrN addrC cos2Dsin2 !eqxx.
Qed.
Lemma RO_is_SO a : RO a \is 'SO[T]_2.
Proof.
by rewrite rotationE RO_is_O /= det_mx22 !mxE /= mulrN opprK -!expr2 cos2Dsin2.
Qed.
Lemma rot2d_helper M a b : sin (b - a) = 1 ->
M = col_mx2 (row2 (cos a) (sin a)) (row2 (cos b) (sin b)) ->
{ a0 | - pi < a0 <= pi & M = RO a0 }.
Proof.
move=> abpi.
have -> : sin b = cos a.
rewrite -[b](subrK a) sinD abpi mul1r [cos (_ - _)]sin1cos0.
by rewrite mul0r addr0.
by rewrite abpi normr1.
have -> : cos b = - sin a.
rewrite -[b](subrK a) cosD abpi mul1r [cos (_ - _)]sin1cos0.
by rewrite mul0r sub0r.
by rewrite abpi normr1.
move=> ->; exists (norm_angle a); first by apply: norm_angle_bound.
by rewrite /RO cos_norm_angle sin_norm_angle.
Qed.
Lemma rot2d M : M \is 'SO[T]_2 -> {a | - pi < a <= pi & M = RO a}.
Proof.
move=> MSO.
move: (MSO); rewrite rotationE => /andP[MO _].
case: (norm1_cossin (norm_row_of_O MO 0)); rewrite !mxE => a [a1 a2].
case: (norm1_cossin (norm_row_of_O MO 1)); rewrite !mxE => b [b1 b2].
move/orthogonalP : (MO) => /(_ 0 1) /=.
rewrite dotmulE sum2E !mxE a1 a2 b1 b2 -cosB => cE.
have : `|sin (a - b)| = 1 by apply: cos0sin1.
case: (ler0P (sin (a - b))) => sE; last first.
exfalso.
move/rotation_det : MSO.
rewrite det_mx22 a1 a2 b1 b2 mulrC -(mulrC (cos b)) -sinB => /esym/eqP.
rewrite -eqr_opp -sinN opprB => /eqP sE1.
by move: sE; rewrite -sE1 ltr_oppr oppr0 (ltr_nat _ 1 0).
rewrite -sinN opprB => /(@rot2d_helper M a b); apply.
by rewrite -a1 -a2 -b1 -b2 [in LHS](col_mx2_rowE M) 2!row2_of_row.
Qed.
Definition RO' a := col_mx2 (row2 (cos a) (sin a)) (row2 (sin a) (- cos a)).
Lemma rot2d_helper' M a b : sin (a - b) = 1 ->
M = col_mx2 (row2 (cos a) (sin a)) (row2 (cos b) (sin b)) ->
{a0 | -pi < a0 <= pi & M = RO' a0}.
Proof.
move=> abpi.
have -> : sin b = - cos a.
rewrite -[a](subrK b) cosD abpi mul1r [cos (_ - _)]sin1cos0.
by rewrite mul0r sub0r opprK.
by rewrite abpi normr1.
have -> : cos b = sin a.
rewrite -[a](subrK b) sinD abpi mul1r [cos (_ - _)]sin1cos0.
by rewrite mul0r addr0.
by rewrite abpi normr1.
move=> ->; exists (norm_angle a); first exact: norm_angle_bound.
by rewrite /RO' cos_norm_angle sin_norm_angle.
Qed.
Lemma rot2d' M :
M \is 'O[T]_2 ->
{a : T & { - pi < a <= pi /\ M = RO a} +
{ - pi < a <= pi /\ M = RO' a}}.
Proof.
move=> MO.
case: (norm1_cossin (norm_row_of_O MO 0)); rewrite !mxE => a [a1 a2].
case: (norm1_cossin (norm_row_of_O MO 1)); rewrite !mxE => b [b1 b2].
move/orthogonalP : (MO) => /(_ 0 1) /=.
rewrite dotmulE sum2E !mxE a1 a2 b1 b2 -cosB.
have HM : M = col_mx2 (row2 (cos a) (sin a)) (row2 (cos b) (sin b)).
by rewrite -a1 -a2 -b1 -b2 [in LHS](col_mx2_rowE M) 2!row2_of_row.
move=> cE.
have : `|sin (a - b)| = 1 by apply: cos0sin1.
case: (ler0P (sin (a - b))) => sE; last first.
case/(@rot2d_helper' M)/(_ HM) => a0.
by exists a0; right.
rewrite -sinN opprB => abpi.
case: (rot2d_helper abpi HM) => a0 KM.
exists a0; by left.
Qed.
Lemma tr_SO2 M : M \is 'SO[T]_2 -> `|\tr M| <= 2%:R.
Proof.
case/rot2d => a aB PRO; move: (cos_max a) => ca.
rewrite PRO tr_RO -(mulr_natr (cos a)) normrM normr_nat.
by rewrite -[in X in _ <= X]mulr_natr ler_pmul.
Qed.
End two_dimensional_rotation.
Section elementary_rotations.
Variable T : realType.
Implicit Types a b : T.
Local Open Scope frame_scope.
Definition Rx a := col_mx3
'e_0
(row3 0 (cos a) (sin a))
(row3 0 (- sin a) (cos a)).
Lemma Rx0 : Rx 0 = 1.
Proof.
by rewrite /Rx cos0 sin0 oppr0; apply/matrix3P/and9P; split; rewrite !mxE.
Qed.
Lemma Rxpi : Rx pi = diag_mx (row3 1 (-1) (-1)).
Proof.
rewrite /Rx cospi sinpi oppr0; apply/matrix3P/and9P; split;
by rewrite !mxE /= -?mulNrn ?mulr1n ?mulr0n.
Qed.
Lemma Rx_RO a : Rx a = block_mx (1 : 'M_1) 0 0 (RO a).
Proof.
rewrite -(@submxK _ 1 2 1 2 (Rx a)) (_ : ulsubmx _ = 1); last first.
apply/rowP => i; by rewrite (ord1 i) !mxE /=.
rewrite (_ : ursubmx _ = 0); last by apply/rowP => i; rewrite !mxE.
rewrite (_ : dlsubmx _ = 0); last first.
apply/colP => i; rewrite !mxE /=.
case: ifPn; [by rewrite !mxE | by case: ifPn; rewrite !mxE].
rewrite (_ : drsubmx _ = RO a) //; by apply/matrix2P; rewrite !mxE /= !eqxx.
Qed.
Lemma Rx_is_SO a : Rx a \is 'SO[T]_3.
Proof. by rewrite Rx_RO (SOSn_SOn 1) RO_is_SO. Qed.
Lemma mxtrace_Rx a : \tr (Rx a) = 1 + cos a *+ 2.
Proof. by rewrite /Rx /mxtrace sum3E !mxE /= -addrA -mulr2n. Qed.
Lemma inv_Rx a : (Rx a)^-1 = Rx (- a).
Proof.
move/rotation_inv : (Rx_is_SO a) => ->.
rewrite /Rx cosN sinN opprK; by apply/matrix3P/and9P; split; rewrite !mxE.
Qed.
Definition Rx' a := col_mx3
'e_0
(row3 0 (cos a) (sin a))
(row3 0 (sin a) (- cos a)).
Lemma Rx'_RO a : Rx' a = block_mx (1 : 'M_1) 0 0 (RO' a).
Proof.
rewrite -(@submxK _ 1 2 1 2 (Rx' a)) (_ : ulsubmx _ = 1); last first.
apply/rowP => i; by rewrite (ord1 i) !mxE /=.
rewrite (_ : ursubmx _ = 0); last by apply/rowP => i; rewrite !mxE.
rewrite (_ : dlsubmx _ = 0); last first.
apply/colP => i; rewrite !mxE /=.
case: ifPn; first by rewrite !mxE.
by case: ifPn; rewrite !mxE.
rewrite (_ : drsubmx _ = RO' a) //; by apply/matrix2P; rewrite !mxE /= !eqxx.
Qed.
Lemma det_Rx' a : \det (Rx' a) = -1.
Proof.
rewrite det_mx33 !mxE /=. Simp.r. by rewrite -!expr2 -opprD cos2Dsin2.
Qed.
Definition Ry a := col_mx3
(row3 (cos a) 0 (- sin a))
'e_1
(row3 (sin a) 0 (cos a)).
Lemma Ry_is_SO a : Ry a \is 'SO[T]_3.
Proof.
apply/rotation3P/and4P; split.
- rewrite -(@eqr_expn2 _ 2) // ?norm_ge0 // expr1n.
by rewrite -dotmulvv dotmulE sum3E !mxE /= mulr0 addr0 -2!expr2 sqrrN cos2Dsin2.
- rewrite -(@eqr_expn2 _ 2) // ?norm_ge0 // expr1n.
by rewrite -dotmulvv dotmulE sum3E !mxE /= mulr0 addr0 add0r mulr1.
- by rewrite 2!rowK /= dotmulE sum3E !mxE /= !mulr0 mul0r !add0r.
- rewrite 3!rowK /= crossmulE !mxE /=. by Simp.r.
Qed.
Definition Rz a := col_mx3
(row3 (cos a) (sin a) 0)
(row3 (- sin a) (cos a) 0)
'e_2%:R.
Lemma Rz_RO a : Rz a = block_mx (RO a) 0 0 (1 : 'M_1).
Proof.
rewrite -(@submxK _ 2 1 2 1 (Rz a)) (_ : drsubmx _ = 1); last first.
apply/rowP => i; by rewrite (ord1 i) !mxE.
rewrite (_ : ulsubmx _ = RO a); last by apply/matrix2P; rewrite !mxE !eqxx.
rewrite (_ : ursubmx _ = 0); last first.
apply/colP => i; case/boolP : (i == 0) => [|/ifnot01P]/eqP->; by rewrite !mxE.
rewrite (_ : dlsubmx _ = 0) //; apply/rowP => i; rewrite !mxE /=.
by case/boolP : (i == 0) => [|/ifnot01P]/eqP->.
Qed.
Lemma trmx_Rz a : (Rz a)^T = Rz (- a).
Proof. by rewrite Rz_RO (tr_block_mx (RO a)) !(trmx0,trmx1) trmx_RO -Rz_RO. Qed.
Lemma RzM a b : Rz a * Rz b = Rz (a + b).
Proof.
rewrite {1 2}/Rz e2row -col_mx3_mul 3!mulmx_row3_col3. Simp.r.
rewrite !row3Z !row3D. Simp.r. rewrite -e2row; congr col_mx3.
- by rewrite -cosD sinD (addrC (_ * _)).
- by rewrite -opprD -sinD [in X in row3 _ X _]addrC -cosD.
Qed.
Lemma Rz_is_SO a : Rz a \is 'SO[T]_3.
Proof.
apply/rotation3P/and4P; split.
- rewrite -(@eqr_expn2 _ 2) // ?norm_ge0 // expr1n.
by rewrite -dotmulvv dotmulE sum3E !mxE /= mulr0 addr0 -2!expr2 cos2Dsin2.
- rewrite -(@eqr_expn2 _ 2) // ?norm_ge0 // expr1n.
- by rewrite -dotmulvv dotmulE sum3E !mxE /= mulr0 addr0 mulrN mulNr opprK addrC cos2Dsin2.
- by rewrite 2!rowK /= dotmulE sum3E !mxE /= mulrN mulr0 addr0 addrC mulrC subrr.
- rewrite 3!rowK /= crossmulE !mxE /=. Simp.r. by rewrite -!expr2 cos2Dsin2 e2row.
Qed.
Lemma RzE a : Rz a = (frame_of_SO (Rz_is_SO a)) _R^ (can_frame T).
Proof. rewrite FromTo_to_can; by apply/matrix3P/and9P; split; rewrite !mxE. Qed.
Lemma rmap_Rz_e0 a :
rmap (can_tframe T) `[ 'e_0 $ frame_of_SO (Rz_is_SO a) ] =
`[ row 0 (Rz a) $ can_tframe T ].
Proof. by rewrite rmapE_to_can rowE [in RHS]RzE FromTo_to_can. Qed.
Definition Rzy a b := col_mx3
(row3 (cos a * cos b) (sin a) (- cos a * sin b))
(row3 (- sin a * cos b) (cos a) (sin a * sin b))
(row3 (sin b) 0 (cos b)).
Lemma RzyE a b : Rz a * Ry b = Rzy a b.
Proof.
rewrite /Rz /Ry -col_mx3_mul; congr col_mx3.
- rewrite mulmx_row3_col3 scale0r addr0 row3Z mulr0.
by rewrite e1row row3Z row3D ?(addr0,mulr0,add0r,mulr1,mulrN,mulNr).
- rewrite mulmx_row3_col3 scale0r addr0 row3Z mulr0.
by rewrite e1row row3Z row3D mulr0 addr0 add0r mulr1 addr0 mulrN !mulNr opprK.
- by rewrite e2row mulmx_row3_col3 scale0r add0r scale0r add0r scale1r.
Qed.
Lemma Rzy_is_SO a b : Rzy a b \is 'SO[T]_3.
Proof. by rewrite -RzyE rpredM//= ?Rz_is_SO // Ry_is_SO. Qed.
End elementary_rotations.
Section isRot.
Local Open Scope frame_scope.
Variable T : realType.
Implicit Types a : T.
Definition isRot a (u : 'rV[T]_3) (f : {linear 'rV_3 -> 'rV_3}) : bool :=
let: j := (Base.frame u) |, 1 in let: k := (Base.frame u) |, 2%:R in
[&& f u == u,
f j == cos a *: j + sin a *: k &
f k == - sin a *: j + cos a *: k].
Lemma isRotP a u (f : {linear 'rV_3 -> 'rV_3}) : reflect
(let: j := (Base.frame u) |, 1 in let: k := (Base.frame u) |, 2%:R in
[/\ f u = u, f j = cos a *: j + sin a *: k & f k = - sin a *: j + cos a *: k])
(isRot a u f).
Proof.
apply: (iffP idP); first by case/and3P => /eqP ? /eqP ? /eqP ?.
by case => H1 H2 H3; apply/and3P; rewrite H1 H2 H3.
Qed.
Section properties_of_isRot.
Variable u : 'rV[T]_3.
Implicit Types M N : 'M[T]_3.
Lemma isRot_axis a M : isRot a u (mx_lin1 M) -> u *m M = u.
Proof. by case/isRotP. Qed.
Lemma isRot1 : isRot 0 u (mx_lin1 1).
Proof.
apply/isRotP; split => /=; first by rewrite mulmx1.
by rewrite cos0 sin0 mulmx1 scale0r addr0 scale1r.
by rewrite mulmx1 sin0 cos0 scaleNr scale0r oppr0 add0r scale1r.
Qed.
Lemma isRotpi (u1 : norm u = 1) : isRot pi u (mx_lin1 (u^T *m u *+ 2 - 1)).
Proof.
apply/isRotP; split => /=.
- by rewrite mulmxBr mulmx1 mulr2n mulmxDr mulmxA dotmul1 // ?mul1mx addrK.
- rewrite cospi sinpi scale0r addr0 scaleN1r -Base.jE ?norm1_neq0 //=.
rewrite mulmxDr -scaler_nat -scalemxAr mulmxA.
by rewrite Base.j_tr_mul // mul0mx scaler0 add0r mulmxN mulmx1.
- rewrite sinpi oppr0 scale0r add0r cospi scaleN1r.
rewrite -Base.kE ?norm1_neq0 //= mulmxBr mulmx1.
by rewrite -scaler_nat -scalemxAr mulmxA Base.k_tr_mul // scaler0 add0r.
Qed.
Lemma isRotD a b M N : isRot a u (mx_lin1 M) -> isRot b u (mx_lin1 N) ->
isRot (a + b) u (mx_lin1 (M * N)).
Proof.
move=> /isRotP[/= H1 H2 H3] /isRotP[/= K1 K2 K3]; apply/isRotP; split => /=.
- by rewrite mulmxA H1 K1.
- rewrite mulmxA H2 mulmxDl cosD sinD -2!scalemxAl K2 K3 2!scalerDr addrACA.
by rewrite !scalerA mulrN -2!scalerDl (addrC (cos a * sin b)).
- rewrite mulmxA H3 mulmxDl -2!scalemxAl K2 K3 2!scalerDr !scalerA sinD cosD.
rewrite addrACA mulrN -2!scalerDl -opprB mulNr opprK (addrC (- _ * _)) mulNr.
by rewrite (addrC (cos a * sin b)).
Qed.
Lemma isRotN a M (u0 : u != 0) :
isRot (- a) u (mx_lin1 M) -> isRot a (- u) (mx_lin1 M).
Proof.
move=> /isRotP [/= H1 H2 H3]; apply/isRotP; split => /=.
by rewrite mulNmx H1.
by rewrite Base.jN (Base.kN u0) H2 cosN sinN scalerN scaleNr.
by rewrite (Base.kN u0) Base.jN mulNmx H3 sinN cosN opprK scalerN scaleNr opprD.
Qed.
Lemma isRotZ a f k (u0 : u != 0) (k0 : 0 < k) :
isRot a (k *: u) f = isRot a u f.
Proof.
rewrite /isRot !Base.Z // !linearZ; congr andb.
apply/idP/idP => [/eqP/scalerI ->//|/eqP ->//]; by move/gt_eqF : k0 => /negbT.
Qed.
Lemma isRotZN a f k (u0 : u != 0) (k0 : k < 0):
isRot a (k *: u) (mx_lin1 f) = isRot (- a) u (mx_lin1 f).
Proof.
rewrite /isRot /= sinN cosN opprK Base.ZN // Base.jN (Base.kN u0).
rewrite !scalerN !scaleNr mulNmx eqr_oppLR opprD !opprK -scalemxAl; congr andb.
apply/idP/idP => [/eqP/scalerI ->//|/eqP ->//]; by move/lt_eqF : k0 => /negbT.
Qed.
Lemma mxtrace_isRot a M (u0 : u != 0) :
isRot a u (mx_lin1 M) -> \tr M = 1 + cos a *+ 2.
Proof.
case/isRotP=> /= Hu Hj Hk.
move: (@basis_change _ M (Base.frame u) (Rx a)).
rewrite /= !mxE /= !scale1r !scale0r !add0r !addr0.
rewrite (invariant_colinear u0 Hu) ?colinear_frame0 // => /(_ erefl Hj Hk) ->.
rewrite mxtrace_mulC mulmxA mulmxV ?mul1mx ?mxtrace_Rx //.
by rewrite unitmxE unitfE rotation_det ?oner_neq0 // Base.is_SO.
Qed.
Lemma same_isRot M N v k (u0 : u != 0) (k0 : 0 < k) a :
u = k *: v ->
isRot a u (mx_lin1 M) -> isRot a v (mx_lin1 N) ->
M = N.
Proof.
move=> mkp /isRotP[/= HMi HMj HMk] /isRotP[/= HNi HNj HNk].
apply/eqP/mulmxP => w.
rewrite (orthogonal_expansion (Base.frame u) w).
rewrite !mulmxDl -!scalemxAl.
have v0 : v != 0 by apply: contra u0; rewrite mkp => /eqP ->; rewrite scaler0.
congr (_ *: _ + _ *: _ + _ *: _).
- by rewrite (Base.frame0E u0) /normalize -scalemxAl HMi {2}mkp -HNi scalemxAl -mkp
scalemxAl.
- by rewrite HMj /= mkp (Base.Z _ k0) -HNj.
- by rewrite HMk /= mkp (Base.Z _ k0) -HNk.
Qed.
Lemma isRot_0_inv (u0 : u != 0) M : isRot 0 u (mx_lin1 M) -> M = 1.
Proof.
move=> H; move/(same_isRot u0 ltr01 _ H) : isRot1; apply; by rewrite scale1r.
Qed.
Lemma isRot_tr a (u0 : u != 0) M : M \in unitmx ->
isRot (- a) u (mx_lin1 M) -> isRot a u (mx_lin1 M^-1).
Proof.
move=> Hf /isRotP /= [/= H1 H2 H3].
move: (@basis_change _ M (Base.frame u) (Rx (- a))).
rewrite /= !mxE /= !(scale0r,addr0,add0r,scale1r) -H2 -H3.
rewrite (invariant_colinear u0 H1) ?colinear_frame0 //.
move/(_ erefl erefl erefl) => fRx.
have HfRx : M^-1 = (col_mx3 (Base.frame u)|,0 (Base.frame u)|,1 (Base.frame u)|,2%:R)^T *m
(Rx (- a))^-1 *m col_mx3 (Base.frame u)|,0 (Base.frame u)|,1 (Base.frame u)|,2%:R.
rewrite fRx invrM /=; last 2 first.
rewrite unitrMr orthogonal_unit // ?(rotation_sub (Rx_is_SO _)) //.
by rewrite rotation_inv ?Base.is_SO // rotation_sub // rotationV Base.is_SO.
by rewrite orthogonal_unit // rotation_sub // Base.is_SO.
rewrite invrM; last 2 first.
rewrite rotation_inv ?Base.is_SO // orthogonal_unit // rotation_sub //.
by rewrite rotationV // Base.is_SO.
by rewrite orthogonal_unit // ?(rotation_sub (Rx_is_SO _)).
by rewrite invrK rotation_inv ?Base.is_SO // mulmxE mulrA.
apply/isRotP; split => /=.
- by rewrite -{1}H1 -mulmxA mulmxV // mulmx1.
- rewrite HfRx !mulmxA.
rewrite (_ : (Base.frame u)|,1 *m _ = 'e_1); last first.
by rewrite mul_tr_col_mx3 dotmulC dotmulvv normj // expr1n idotj // jdotk // e1row.
rewrite (_ : 'e_1 *m _ = row3 0 (cos (- a)) (sin a)); last first.
rewrite (rotation_inv (Rx_is_SO (- a))) /Rx mul_tr_col_mx3.
rewrite dote2 /= 2!dotmulE 2!sum3E !mxE /= cosN sinN opprK. by Simp.r.
by rewrite mulmx_row3_col3 scale0r add0r cosN.
- rewrite HfRx !mulmxA.
rewrite (_ : (Base.frame u)|,2%:R *m _ = 'e_2%:R); last first.
by rewrite mul_tr_col_mx3 dotmulC idotk // dotmulC jdotk // dotmulvv normk // expr1n e2row.
rewrite (_ : 'e_2%:R *m _ = row3 0 (- sin a) (cos a)); last first.
rewrite (rotation_inv (Rx_is_SO (- a))) /Rx mul_tr_col_mx3.
rewrite dote2 /= 2!dotmulE 2!sum3E !mxE /= cosN sinN opprK. by Simp.r.
by rewrite mulmx_row3_col3 scale0r add0r.
Qed.
Lemma isRot_SO a M (u0 : u != 0) : isRot a u (mx_lin1 M) -> M \is 'SO[T]_3.
Proof.
move/isRotP=> /= [Hu Hj Hk].
move: (@basis_change _ M (Base.frame u) (Rx a)).
rewrite /= !mxE /= !(scale1r,scale0r,add0r,addr0).
rewrite (invariant_colinear u0 Hu) ?colinear_frame0 // => /(_ erefl Hj Hk) ->.
by rewrite rpredM // ?Base.is_SO // rpredM // ?Rx_is_SO // rotation_inv // ?Base.is_SO // rotationV Base.is_SO.
Qed.
End properties_of_isRot.
Section relation_with_rotation_matrices.
Lemma SO_isRot M : M \is 'SO[T]_3 ->
{a | - pi < a <= pi & isRot a (vaxis_euler M) (mx_lin1 M)}.
Proof.
move=> MSO.
set e := vaxis_euler M.
case/boolP : (M == 1) => [/eqP ->|M1].
exists 0; last by exact: isRot1.
by rewrite oppr_cp0 ?(pi_ge0, pi_gt0).
have v0 := vaxis_euler_neq0 MSO.
rewrite -/e in v0.
have vMv := vaxis_eulerP MSO.
rewrite -/e in vMv.
set i := (Base.frame e)|,0. set j := (Base.frame e)|,1. set k := (Base.frame e)|,2%:R.
have iMi : i *m M = i.
by rewrite (invariant_colinear v0) // ?colinear_frame0.
have iMj : i *d (j *m M) = 0.
rewrite -iMi (proj2 (orth_preserves_dotmul M) (rotation_sub MSO) i j).
by rewrite /i /j 2!rowframeE dot_row_of_O // NOFrame.MO.
have iMk : i *d (k *m M) = 0.
rewrite -iMi (proj2 (orth_preserves_dotmul M) (rotation_sub MSO) i k).
by rewrite /i /k 2!rowframeE dot_row_of_O // NOFrame.MO.
set a := (j *m M) *d j.
set b := (j *m M) *d k.
have ab : j *m M = a *: j + b *: k.
by rewrite {1}(orthogonal_expansion (Base.frame e) (j *m M)) dotmulC iMj
scale0r add0r.
set c := (k *m M) *d j.
set d := (k *m M) *d k.
have cd : k *m M = c *: j + d *: k.
by rewrite {1}(orthogonal_expansion (Base.frame e) (k *m M)) dotmulC iMk
scale0r add0r.
have H1 : a ^+ 2 + b ^+ 2 = 1.
move/eqP: (norm_row_of_O (NOFrame.MO (Base.frame e)) 1).
rewrite -(@eqr_expn2 _ 2) // ?norm_ge0 // expr1n -dotmulvv.
rewrite -(proj2 (orth_preserves_dotmul M) (rotation_sub MSO)).
rewrite -rowframeE ab dotmulDr 2!dotmulDl 4!dotmulvZ 4!dotmulZv 2!dotmulvv.
by rewrite normj // normk // !(expr1n,mulr1) -!expr2 dotmulC jdotk // !(mulr0,add0r,addr0) => /eqP.
have H2 : a * c + b * d = 0.
move/eqP: (dot_row_of_O (NOFrame.MO (Base.frame e)) 1 2%:R).
rewrite -2!rowframeE -(proj2 (orth_preserves_dotmul M) (rotation_sub MSO) j k) ab cd.
rewrite dotmulDr 2!dotmulDl 4!dotmulvZ 4!dotmulZv 2!dotmulvv normj // normk //.
by rewrite expr1n !mulr1 dotmulC jdotk // 4!mulr0 add0r addr0 mulrC (mulrC d) => /eqP.
have H3 : c ^+ 2 + d ^+ 2 = 1.
move/eqP: (norm_row_of_O (NOFrame.MO (Base.frame e)) 2%:R).
rewrite -(@eqr_expn2 _ 2) // ?norm_ge0 // expr1n -dotmulvv.
rewrite -(proj2 (orth_preserves_dotmul M) (rotation_sub MSO)) -!rowframeE -/k cd.
rewrite dotmulDr 2!dotmulDl 4!dotmulvZ 4!dotmulZv 2!dotmulvv normj // normk //.
by rewrite expr1n 2!mulr1 -2!expr2 dotmulC jdotk // !(mulr0,addr0,add0r) => /eqP.
set P := col_mx2 (row2 a b) (row2 c d).
have PO : P \is 'O[T]_2.
apply/orthogonal2P; rewrite !rowK /= !dotmulE !sum2E !mxE /=.
by rewrite -!expr2 H1 H2 mulrC (mulrC d) H2 H3 !eqxx.
case: (rot2d' PO) => phi [[phiB phiRO] | [phiB phiRO']]; subst P.
- case/eq_col_mx2 : phiRO => Ha Hb Hc Hd.
exists phi => //.
rewrite -(@isRotZ _ phi (mx_lin1 M) 1 _ _) // scale1r; apply/isRotP; split => //.
by rewrite -!(Ha,Hb,Hc).
by rewrite -!(Hb,Hc,Hd).
- exfalso.
case/eq_col_mx2 : phiRO' => Ha Hb Hc Hd.
move: (@basis_change _ M (Base.frame e) (Rx' phi)).
rewrite !mxE /= !(addr0,add0r,scale0r,scale1r) -/i -/j -/k.
rewrite -{1}Ha -{1}Hb -{1}Hc -{1}Hd.
rewrite -ab iMi -cd => /(_ erefl erefl erefl) => HM.
move: (rotation_det MSO).
rewrite HM 2!det_mulmx det_Rx' detV -crossmul_triple.
move: (Frame.MSO (Base.frame e)).
rewrite rotationE => /andP[_] /=.
rewrite crossmul_triple => /eqP.
rewrite /i /j /k.
rewrite !rowframeE.
rewrite col_mx3_row => ->.
rewrite invr1 mulr1 mul1r => /eqP.
by rewrite eqrNxx oner_eq0.
Qed.
End relation_with_rotation_matrices.
End isRot.
Section exponential_map_rot.
Variable T : realType.
Let vector := 'rV[T]_3.
Implicit Types (u v w : vector) (a b : T) (M : 'M[T]_3).
Definition emx3 a M : 'M_3 := 1 + sin a *: M + (1 - cos a) *: M ^+ 2.
Local Notation "'`e(' a ',' M ')'" := (emx3 a M).
Lemma emx3a0 a : `e(a, 0) = 1.
Proof. by rewrite /emx3 expr0n /= 2!scaler0 2!addr0. Qed.
Lemma emx30M M : `e(0, M) = 1.
Proof. by rewrite /emx3 sin0 cos0 subrr 2!scale0r 2!addr0. Qed.
Lemma emx30M' M a : cos a = 1 -> sin a = 0 -> `e(a, M) = 1.
Proof.
by rewrite /emx3 => -> ->; rewrite subrr 2!scale0r 2!addr0.
Qed.
Lemma tr_emx3 a M : `e(a, M)^T = `e(a, M^T).
Proof.
by rewrite /emx3 !linearD /= !linearZ /= trmx1 expr2 trmx_mul expr2.
Qed.
Lemma emx3M a b M : M ^+ 3 = - M -> `e(a, M) * `e(b, M) = `e(a + b, M).
Proof.
move=> cube_u.
rewrite /emx3 sinD cosD !mulrDr !mulrDl.
Simp.r => /=.
rewrite -scalerCA -2!scalerAl -expr2.
rewrite -scalerAl -scalerAr -exprSr cube_u (scalerN (sin b) M) (scalerN (1 - cos a)).
rewrite -(scalerAl (sin a)) -(scalerCA (1 - cos b) M) -(scalerAl (1 - cos b)) -exprS.
rewrite cube_u (scalerN _ M) (scalerN (sin a) (_ *: _)).
rewrite -!addrA; congr (_ + _).
do 2 rewrite addrC -!addrA.
rewrite addrC scalerA (mulrC (sin b)) -!addrA.
rewrite [in RHS]addrC [in RHS]scalerBl [in RHS]scalerBl [in RHS]opprB [in RHS]addrCA -![in RHS]addrA; congr (_ + _).
rewrite scalerBl scale1r opprB (scalerA (cos a)) -!addrA.
rewrite [in RHS]scalerDl ![in RHS]addrA [in RHS]addrC -[in RHS]addrA; congr (_ + _).
rewrite addrC ![in LHS]addrA addrK.
rewrite -![in LHS]addrA addrC scalerBl scale1r scalerBr opprB scalerA -![in LHS]addrA.
rewrite [in RHS]addrA [in RHS]addrC; congr (_ + _).
rewrite addrCA ![in LHS]addrA subrK -scalerCA -2!scalerAl -exprD.
rewrite (_ : M ^+ 4 = - M ^+ 2); last by rewrite exprS cube_u mulrN -expr2.
rewrite 2!scalerN scalerA.
rewrite addrC -scaleNr -2!scalerDl -scalerBl; congr (_ *: _).
rewrite -!addrA; congr (_ + _).
rewrite mulrBr mulr1 mulrBl mul1r opprB opprB !addrA subrK addrC.
rewrite -(addrC (cos a)) !addrA -(addrC (cos a)) subrr add0r.
by rewrite addrC addrA subrr add0r mulrC.
Qed.
Lemma inv_emx3 a M : M ^+ 4 = - M ^+ 2 -> `e(a, M) * `e(a, - M) = 1.
Proof.
move=> aM.
case/boolP : (cos a == 1) => [/eqP|] ca; rewrite /emx3.
rewrite ca subrr (_ : sin a = 0) ; last by rewrite cos1sin0 // ca normr1.
by rewrite !scale0r !addr0 mulr1.
rewrite !mulrDr !mulrDl !mulr1 !mul1r -[RHS]addr0 -!addrA; congr (_ + _).
rewrite !addrA sqrrN -!addrA (addrCA (_ *: M ^+ 2)) !addrA scalerN subrr add0r.
rewrite (_ : (1 - _) *: _ * _ = - (sin a *: M * ((1 - cos a) *: M ^+ 2))); last first.
rewrite mulrN; congr (- _).
by rewrite -2!scalerAr -!scalerAl -exprS -exprSr 2!scalerA mulrC.
rewrite -!addrA (addrCA (- (sin a *: _ * _))) !addrA subrK.
rewrite mulrN -scalerAr -scalerAl -expr2 scalerA -expr2.
rewrite -[in X in _ - _ + _ + X = _]scalerAr -scalerAl -exprD scalerA -expr2.
rewrite -scalerBl -scalerDl sin2cos2.
rewrite -{2}(expr1n _ 2) subr_sqr -{1 3}(mulr1 (1 - cos a)) -mulrBr -mulrDr.
rewrite opprD addrA subrr add0r -(addrC 1) -expr2 -scalerDr.
apply/eqP; rewrite scaler_eq0 sqrf_eq0 subr_eq0 eq_sym (negbTE ca) /=.
by rewrite aM subrr.
Qed.
Local Notation "'`e^(' a ',' w ')'" := (emx3 a \S( w )).
Lemma eskew_pi w : norm w = 1 -> `e^(pi, w) = w^T *m w *+ 2 - 1.
Proof.
move=> w1.
rewrite /emx3 sinpi scale0r addr0 cospi opprK -(natrD _ 1 1).
rewrite sqr_spin w1 expr1n scalerDr addrCA scalerN scaler_nat; congr (_ + _).
rewrite scaler_nat mulr2n opprD addrCA.
by rewrite (_ : 1%:A = 1) // ?subrCA ?subrr ?addr0 // -idmxE scale1r.
Qed.
Lemma eskew_pi' w a :
norm w = 1 -> cos a = -1 -> sin a = 0 -> `e^(a, w) = w^T *m w *+ 2 - 1.
Proof.
move=> w1 Hs Hc.
rewrite /emx3 Hs Hc scale0r addr0 opprK -(natrD _ 1 1).
rewrite sqr_spin w1 expr1n scalerDr addrCA scalerN scaler_nat; congr (_ + _).
rewrite scaler_nat mulr2n opprD addrCA.
by rewrite (_ : 1%:A = 1) // ?subrCA ?subrr ?addr0 // -idmxE scale1r.
Qed.
Lemma eskew_v0 a : `e^(a, 0) = 1.
Proof. by rewrite spin0 emx3a0. Qed.
Lemma unspin_eskew a w : unspin `e^(a, w) = sin a *: w.
Proof.
rewrite /emx3 !(unspinD,unspinZ,unspinN,sqr_spin,spinK,unspin_cst,scaler0,add0r,subr0).
by rewrite unspin_sym ?scaler0 ?addr0 // mul_tr_vec_sym.
Qed.
Lemma tr_eskew a w : `e^(a, w)^T = `e^(a, - w).
Proof. by rewrite tr_emx3 tr_spin /emx3 spinN. Qed.
Lemma eskewM a b w : norm w = 1 -> `e^(a, w) * `e^(b, w) = `e^(a + b, w).
Proof. move=> w1; by rewrite emx3M // spin3 w1 expr1n scaleN1r. Qed.
Lemma trace_eskew a w : norm w = 1 -> \tr `e^(a, w) = 1 + 2%:R * cos a.
Proof.
move=> w1.
rewrite 2!mxtraceD !mxtraceZ /= mxtrace1.
rewrite (trace_anti (spin_is_so w)) mulr0 addr0 mxtrace_sqr_spin w1.
rewrite (_ : - _ = - 2%:R); last by rewrite expr1n mulr1.
by rewrite mulrDl addrA mul1r -natrB // mulrC mulrN -mulNr opprK.
Qed.
(* table 1.1 of [springer]
'equivalent rotation matrices for various representations of orientation'
angle-axis angle a, vector u *)
Definition angle_axis_rot a u :=
let va := 1 - cos a in let ca := cos a in let sa := sin a in
col_mx3
(row3 (u``_0 ^+2 * va + ca)
(u``_0 * u``_1 * va + u``_2%:R * sa)
(u``_0 * u``_2%:R * va - u``_1 * sa))
(row3 (u``_0 * u``_1 * va - u``_2%:R * sa)
(u``_1 ^+2 * va + ca)
(u``_1 * u``_2%:R * va + u``_0 * sa))
(row3 (u``_0 * u``_2%:R * va + u``_1 * sa)
(u``_1 * u``_2%:R * va - u``_0 * sa)
(u``_2%:R ^+2 * va + ca)).
Lemma eskewE a u : norm u = 1 -> `e^(a, u) = angle_axis_rot a u.
Proof.
pose va := 1 - cos a. pose ca := cos a. pose sa := sin a.
move=> w1; apply/matrix3P/and9P; split; apply/eqP.
- rewrite 2![in RHS]mxE /= [in LHS]mxE -/sa -/va 3!mxE /= !spinij; Simp.r => /=.
rewrite sqr_spin' !mxE /=.
rewrite (_ : - _ - _ = u``_0 ^+ 2 - 1); last first.
rewrite -[in X in _ = _ - X](expr1n _ 2%N) -w1 -dotmulvv dotmulE sum3E -3!expr2.
by rewrite !opprD !addrA subrr add0r addrC.
- rewrite mulrBr mulr1 addrCA mulrC; congr (_ + _).
by rewrite /va opprB addrC subrK.
- rewrite 2![in RHS]mxE /= [in LHS]mxE -/sa -/va 3!mxE /= !spinij; Simp.r => /=.
by rewrite sqr_spin' !mxE /= addrC mulrC (mulrC sa).
- rewrite 2![in RHS]mxE /= [in LHS]mxE -/sa -/va 3!mxE /= !spinij; Simp.r => /=.
by rewrite sqr_spin' !mxE /= addrC mulrC (mulrC sa).
- rewrite 2![in RHS]mxE /= [in LHS]mxE -/sa -/va 3!mxE /= !spinij; Simp.r => /=.
by rewrite sqr_spin' !mxE /= addrC mulrC (mulrC sa).
- rewrite 2![in RHS]mxE /= [in LHS]mxE -/sa -/va 3!mxE /= !spinij; Simp.r => /=.
rewrite sqr_spin' !mxE /=.
rewrite (_ : - _ - _ = u``_1 ^+ 2 - 1); last first.
rewrite -[in X in _ = _ - X](expr1n _ 2%N) -w1 -dotmulvv dotmulE sum3E -3!expr2.
by rewrite 2!opprD addrCA addrA subrK addrC.
rewrite mulrBr mulr1 addrCA mulrC; congr (_ + _).
by rewrite /va opprB addrC subrK.
- rewrite 2![in RHS]mxE /= [in LHS]mxE -/sa -/va 3!mxE /= !spinij; Simp.r => /=.
by rewrite sqr_spin' !mxE /= addrC mulrC (mulrC sa).
- rewrite 2![in RHS]mxE /= [in LHS]mxE -/sa -/va 3!mxE /= !spinij; Simp.r => /=.
by rewrite sqr_spin' !mxE /= addrC mulrC (mulrC sa).
- rewrite 2![in RHS]mxE /= [in LHS]mxE -/sa -/va 3!mxE /= !spinij; Simp.r => /=.
by rewrite sqr_spin' !mxE /= addrC mulrC (mulrC sa).
- rewrite 2![in RHS]mxE /= [in LHS]mxE -/sa -/va 3!mxE /= !spinij; Simp.r => /=.
rewrite sqr_spin' !mxE /=.
rewrite (_ : - _ - _ = u``_2%:R ^+ 2 - 1); last first.
rewrite -[in X in _ = _ - X](expr1n _ 2%N) -w1 -dotmulvv dotmulE sum3E -3!expr2.
by rewrite 2!opprD [in RHS]addrC subrK addrC.
rewrite mulrBr mulr1 addrCA mulrC; congr (_ + _).
by rewrite /va opprB addrC subrK.
Qed.
Lemma eskew_is_O a u : norm u = 1 -> `e^(a, u) \is 'O[T]_3.
Proof.
move=> u1.
by rewrite orthogonalE tr_emx3 tr_spin inv_emx3 // exp_spin u1 expr1n scaleN1r.
Qed.
Lemma rank_eskew a w : norm w = 1 -> \rank `e^(a, w) = 3%N.
Proof.
move=> w1; by rewrite mxrank_unit // orthogonal_unit // eskew_is_O.
Qed.
Lemma det_eskew a w : norm w = 1 -> \det `e^(a, w) = 1.
Proof.
move=> w1.
move/orthogonal_det/eqP : (eskew_is_O (a / 2%:R) w1).
rewrite -(@eqr_expn2 _ 2) // expr1n sqr_normr expr2 -det_mulmx.
rewrite mulmxE emx3M; last by rewrite spin3 w1 expr1n scaleN1r.
by move/eqP; rewrite -splitr.
Qed.
Lemma eskew_is_SO a w : norm w = 1 -> `e^(a, w) \is 'SO[T]_3.
Proof. by move=> w1; rewrite rotationE eskew_is_O //= det_eskew. Qed.
Definition expi a := (cos a +i* sin a)%C.
Lemma expiD a b : expi (a + b) = expi a * expi b.
Proof.
rewrite /expi cosD sinD.
by apply/eqP/andP; rewrite eqxx addrC eqxx.
Qed.
Lemma expi0 : expi (0) = 1 :> T[i].
Proof. by rewrite /expi cos0 sin0. Qed.
Definition eskew_eigenvalues a : seq T[i] := [:: 1; expi a; expi (- a)].
From mathcomp Require Import ssrAC.
Lemma eigenvalue_ekew a w : norm w = 1 ->
eigenvalue (map_mx (fun x => x%:C%C) `e^(a, w)) =1
[pred k | k \in eskew_eigenvalues a].
Proof.
move=> u1 /= k.
rewrite inE eigenvalue_root_char -map_char_poly.
rewrite /= !inE.
rewrite char_poly3 /= trace_eskew // det_eskew //.
rewrite [`e(_,_) ^+ _]expr2 eskewM // trace_eskew //.
rewrite (_ : _ - _ = (1 + cos a *+ 2) *+ 2); last first.
rewrite opprD addrA cosD -2!expr2 sin2cos2 opprB addrA -mulr2n mulrDr mulrN1.
rewrite opprB addrA -(addrA _ (-1)) (_ : -1 + 2 = 1)//; last first.
by rewrite addrC; apply/eqP; rewrite subr_eq.
rewrite -addrA mulr_natl; apply/eqP.
rewrite eq_sym [eqbRHS]addrC -subr_eq.
rewrite expr2 mulr2n -[in X in X - _ == _](mulr1 (1 + cos a *+ 2)).
rewrite -mulrDr -mulrBr opprD [in X in _ * X == _]addrA addrK.
rewrite mulrC -subr_sqr expr1n; apply/eqP; congr (1 - _).
by rewrite mulr_natl -mulrnA exprMn_n.
rewrite -[(_ + _) *+ _]mulr_natl mulrA divfK ?(eqr_nat _ 2 0) // mul1r.
rewrite linearB /= map_polyC /= !(linearB, linearD, linearZ) /=.
rewrite !map_polyXn map_polyX.
rewrite (_ : _ - _ = ('X - 1) * ('X - (expi a)%:P) * ('X - (expi (-a))%:P)).
by rewrite !rootM !root_XsubC orbA.
have expiDexpiN : expi a + expi (-a) = (cos a + cos a)%:C%C.
rewrite /expi cosN sinN.
by apply/eqP; rewrite eq_complex /= subrr !eqxx.
rewrite !(mulrBr, mulrBl, mulrDr, mulrDl, mul1r, mulr1).
rewrite -expr2 -exprSr !addrA !scalerN.
rewrite ['X * _ * 'X]mulrAC -expr2 !['X * _]mulrC !['X^2 * _]mulrC.
rewrite [_* 'X * _]mulrAC -rmorphM /= -expiD subrr expi0 mul1r.
rewrite -!addrA; congr (_ + _); rewrite !(addrA, opprB, opprD).
rewrite [in RHS](ACl (1 * 3 * 7 * 2 * 4 * 5 * 6))/=.
rewrite -(addrA (- 'X^2)) -opprD -mulrDl -rmorphD/= expiDexpiN.
rewrite -addrA -3![in RHS]addrA; congr (_ + _).
by rewrite !rmorphD/= !scalerDl/= scale1r !opprD mulrDl opprD/= addrA mul_polyC.
rewrite !addrA [in RHS](ACl (1 * 2 * 4 * 3))/=.
congr (_ - _).
rewrite [RHS]addrAC -mulrDl -rmorphD/= expiDexpiN.
rewrite -(addrA 1) rmorphD/= scalerDl scale1r addrC; congr (_ + _).
by rewrite mul_polyC.
Qed.
Lemma Rz_eskew a : Rz a = `e^(a, 'e_2%:R).
Proof.
rewrite /Rz eskewE /angle_axis_rot ?norm_delta_mx //.
rewrite !mxE /= expr0n /=. Simp.r.
by rewrite expr1n mul1r subrK -e2row.
Qed.
(* the w vector of e(phi,w) is an axis *)
Lemma axial_eskew a w : axial `e^(a, w) = sin a *+ 2 *: w.
Proof.
rewrite axialE unspinD unspin_eskew tr_eskew unspinN unspin_eskew scalerN.
by rewrite opprK -mulr2n scalerMnl.
Qed.
Section rodrigues_formula.
Definition rodrigues u a w :=
u - (1 - cos a) *: (norm w ^+ 2 *: u) + (1 - cos a) * (u *d w) *: w + sin a *: (w *v u).
Lemma rodriguesP u a w : rodrigues u a w = u *m `e^(a, w).
Proof.
rewrite /rodrigues.
rewrite addrAC !mulmxDr mulmx1 -!scalemxAr mulmxA !spinE -!addrA; congr (_ + _).
rewrite !addrA.
rewrite [in X in _ = _ + X](@lieC _ (vec3 T)) scalerN.
rewrite [in X in _ = _ - X](@lieC _ (vec3 T)) /=.
rewrite double_crossmul dotmulvv.
rewrite scalerN opprK.
rewrite scalerBr [in RHS]addrA [in RHS]addrC -!addrA; congr (_ + (_ + _)).
by rewrite dotmulC scalerA.
Qed.
Definition rodrigues_unit u a w :=
cos a *: u + (1 - cos a) * (u *d w) *: w + sin a *: (w *v u).
Lemma rodrigues_unitP u a w : norm w = 1 -> rodrigues_unit u a w = u *m `e^(a, w).
Proof.
move=> w1; rewrite -(rodriguesP u a w).
rewrite /rodrigues /rodrigues_unit w1 expr1n scale1r; congr (_ + _ + _).
by rewrite scalerBl opprB scale1r addrCA addrA addrK.
Qed.
End rodrigues_formula.
Lemma isRot_eskew_normalize a w : w != 0 -> isRot a w (mx_lin1 `e^(a, normalize w)).
Proof.
move=> w0.
pose f := Base.frame w.
apply/isRotP; split => /=.
- rewrite -rodriguesP // /rodrigues (norm_normalize w0) expr1n scale1r.
rewrite dotmul_normalize_norm scalerA -mulrA divrr ?mulr1 ?unitfE ?norm_eq0 //.
by rewrite subrK linearZl_LR /= (@liexx _ (vec3 T)) 2!scaler0 addr0.
- rewrite -rodriguesP // /rodrigues dotmulC norm_normalize // expr1n scale1r.
rewrite (_ : normalize w = Base.i w) (*NB: lemma?*); last by rewrite /Base.i (negbTE w0).
rewrite -Base.jE -Base.kE.
rewrite Base.idotj // mulr0 scale0r addr0 -Base.icrossj /= scalerBl scale1r.
by rewrite opprB addrCA subrr addr0.
- rewrite -rodriguesP /rodrigues dotmulC norm_normalize // expr1n scale1r.
rewrite (_ : normalize w = Base.i w) (*NB: lemma?*); last by rewrite /Base.i (negbTE w0).
rewrite -Base.jE -Base.kE.
rewrite Base.idotk // mulr0 scale0r addr0 scalerBl scale1r opprB addrCA subrr.
rewrite addr0 addrC; congr (_ + _).
by rewrite -/(Base.i w) Base.icrossk // scalerN scaleNr.
Qed.
Lemma isRot_eskew a w w' : normalize w' = w -> norm w = 1 -> isRot a w' (mx_lin1 `e^(a, w)).
Proof.
move=> <- w1.
by rewrite isRot_eskew_normalize // -normalize_eq0 -norm_eq0 w1 oner_eq0.
Qed.
Lemma eskew_is_onto_SO M : M \is 'SO[T]_3 ->
{ a | - pi < a <= pi & M = `e^(a, normalize (vaxis_euler M)) }.
Proof.
move=> MSO.
set w : vector := normalize _.
case: (SO_isRot MSO) => a aB Ha.
exists a => //.
apply: (@same_isRot _ _ _ _ _ (norm (vaxis_euler M)) _ _ _ _ Ha); last first.
apply (@isRot_eskew _ _ w).
by rewrite normalizeI // norm_normalize // vaxis_euler_neq0.
by rewrite norm_normalize // vaxis_euler_neq0.
by rewrite norm_scale_normalize.
by rewrite norm_gt0 vaxis_euler_neq0.
by rewrite vaxis_euler_neq0.
Qed.
Section alternative_definition_of_eskew.
(* rotation of angle a around (unit) vector e *)
Definition eskew_unit (a : T) (e : 'rV[T]_3) :=
e^T *m e + (cos a) *: (1 - e^T *m e) + sin a *: \S( e ).
Lemma eskew_unitE w (a : T) : norm w = 1 -> eskew_unit a w = `e^(a, w).
Proof.
move=> w1.
rewrite /eskew_unit /emx3 addrAC sqr_spin -addrA addrCA.
rewrite -[in RHS]addrA [in RHS]addrCA; congr (_ + _).
rewrite scalerBl scale1r addrCA -addrA; congr (_ + _).
rewrite scalerBr [in RHS]scalerBr opprB !addrA; congr (_ - _).
by rewrite addrC w1 expr1n !scalemx1 (addrC _ 1) subrr addr0.
Qed.
Local Open Scope frame_scope.
(* TODO: move? *)
Lemma normalcomp_double_crossmul p (e : 'rV[T]_3) : norm e = 1 ->
normalcomp p e *v ((Base.frame e)|,2%:R *v (Base.frame e)|,1) = e *v p.
Proof.
move=> u1.
rewrite 2!rowframeE (@lieC _ (vec3 T) (row _ _)) /= SO_jcrossk; last first.
by rewrite -(col_mx3_row (NOFrame.M (Base.frame e))) -!rowframeE Base.is_SO.
rewrite -rowframeE Base.frame0E ?norm1_neq0 //.
rewrite normalizeI // {2}(axialnormalcomp p e) linearD /=.
by rewrite crossmul_axialcomp add0r (@lieC _ (vec3 T)) /= linearNl opprK.
Qed.
Lemma normalcomp_mulO' a Q u p : norm u = 1 -> isRot a u (mx_lin1 Q) ->
normalcomp p u *m Q = cos a *: normalcomp p u + sin a *: (u *v p).
Proof.
move=> u1 H.
set v := normalcomp p u.
move: (orthogonal_expansion (Base.frame u) v).
set p0 := _|,0. set p1 := _|,1. set p2 := _|,2%:R.
rewrite (_ : (v *d p0) *: _ = 0) ?add0r; last first.
by rewrite /p0 Base.frame0E ?norm1_neq0 // normalizeI // dotmul_normalcomp scale0r.
move=> ->.
rewrite mulmxDl -2!scalemxAl.
case/isRotP : H => /= _ -> ->.
rewrite -/p1 -/p2.
rewrite (scalerDr (normalcomp p u *d p1)) scalerA mulrC -scalerA.
rewrite [in RHS]scalerDr -!addrA; congr (_ + _).
rewrite (scalerDr (normalcomp p u *d p2)) addrA addrC.
rewrite scalerA mulrC -scalerA; congr (_ + _).
rewrite scalerA mulrC -scalerA [in X in _ + X = _]scalerA mulrC -scalerA.
rewrite scaleNr -scalerBr; congr (_ *: _).
by rewrite -double_crossmul normalcomp_double_crossmul.
Qed.
(* [angeles] p.42, eqn 2.49 *)
Lemma isRot_eskew_unit_inv a Q u : norm u = 1 ->
isRot a u (mx_lin1 Q) -> Q = eskew_unit a u.