OrienRepr.Quaternion

Require Export MathBase.
Require Export AxisAngle.

Scope for quaternion

Declare Scope quat_scope.
Delimit Scope quat_scope with quat.
Open Scope quat_scope.

Definition of Quaternion

A quaternion q = w + x i + y j + z k, can be considered as a linear combination with the basis of {1, i, j, k}

Definition quat := vec 4.
Bind Scope quat_scope with quat.

Notation "q .W" := (q.1) (at level 30, format "q .W") : quat_scope.
Notation "q .X" := (q.2) (at level 30, format "q .X") : quat_scope.
Notation "q .Y" := (q.3) (at level 30, format "q .Y") : quat_scope.
Notation "q .Z" := (q.4) (at level 30, format "q .Z") : quat_scope.

Get the component of a given quaternion number q

Make a quaternion from real part

Definition s2q (w : R) : quat := l2v [w ;0;0;0].

Lemma s2q_spec : forall w,
let q := s2q w in
q.W = w /\ q.X = R0 /\ q.Y = R0 /\ q.Z = R0.
Proof. intros. cbv. auto. Qed.

Make a quaternion from real part and imaginary part

Definition si2q (w : R) (v : vec 3) : quat := vconsH w v.

Lemma si2q_spec : forall w v,
let q := si2q w v in
q.W = w /\ q.X = v.1 /\ q.Y = v.2 /\ q.Z = v.3.
Proof. intros. v2e v. cbv in *. rewrite Ha. auto. Qed.

si2q is injective

Lemma si2q_inj : forall w1 w2 v1 v2,
si2q w1 v1 = si2q w2 v2 -> w1 = w2 /\ v1 = v2.
Proof. intros. unfold si2q in H. apply vconsH_inj in H. auto. Qed.

Convert between pure imaginary quanternion and 3-dim vector

Definition q2im (q : quat) : vec 3 := vremoveH q.
Notation "q .Im" := (q2im q) (at level 30, format "q .Im") : quat_scope.

Definition im2q (v : vec 3) : quat := vconsH 0 v.

q2im (im2q v) = v

Lemma q2im_im2q : forall v, q2im (im2q v) = v.
Proof. intros. v2e v. veq. Qed.

q.W = 0 -> im2q (q2im q) = q

Lemma im2q_q2im : forall q, q.W = 0 -> im2q (q2im q) = q.
Proof. intros. v2e q. cbv in H. veq. auto. Qed.

Lemma qeq_if_W_Im : forall q1 q2, q1.W = q2.W -> q1.Im = q2.Im -> q1 = q2.
Proof. intros. unfold q2im in *. apply vremoveH_inj in H0; auto. Qed.

Automation for quaternion

Ltac q2e q :=
unfold quat in *;
v2e q.

Quaternion operations

Zero quaternion 零四元数

Make a quaternion with all components are 0

Definition qzero : quat := vzero.

im2q v = qzero <-> v = vzero

Lemma im2q_eq0_iff : forall v, im2q v = qzero <-> v = vzero.
Proof.
  intros. unfold im2q, qzero. logic.
  - replace vzero with (@vconsH 3 0 vzero) in H by veq.
    apply vconsH_inj in H. logic.
  - rewrite H. veq.
Qed.

im2q v <> qzero <-> v <> vzero

Lemma im2q_neq0_iff : forall v, im2q v <> qzero <-> v <> vzero.
Proof. intros. rewrite im2q_eq0_iff. tauto. Qed.

Square of magnitude (Length) of a quaternion

Get square of magnitude (length) of a quaternion

Definition qlen2 (q : quat) : R :=
q.W * q.W + q.X * q.X + q.Y * q.Y + q.Z * q.Z.

0 <= qlen2 q

Lemma qlen2_ge0 : forall (q : quat), (0 <= qlen2 q)%R.
Proof. intros. q2e q. unfold qlen2. ra. Qed.

q = qzero <-> qlen2 q = 0

Lemma qlen2_eq0_iff : forall q : quat, qlen2 q = 0 <-> q = qzero.
Proof. intros. q2e q. rewrite v4eq_iff. cbv. ra. Qed.

q <> qzero <-> qlen2 q <> 0

Lemma qlen2_neq0_iff : forall q : quat, qlen2 q <> 0 <-> q <> qzero.
Proof. intros. rewrite qlen2_eq0_iff. tauto. Qed.

Magnitude (Length) of a quaternion

Get magnitude (length) of a quaternion

Definition qlen (q : quat) : R := sqrt (qlen2 q).
Notation "|| q ||" := (qlen q) : quat_scope.

(||q||)² = qlen2 q

Lemma sqr_qlen : forall q : quat, (||q ||)² = qlen2 q.
Proof. intros. unfold qlen. rewrite Rsqr_sqrt; ra. unfold qlen2. ra. Qed.

0 <= ||q||

Lemma qlen_ge0 : forall q : quat, 0 <= ||q ||.
Proof. intros. unfold qlen. ra. Qed.

|| q || = 0 <-> q = qzero

Lemma qlen_eq0_iff : forall q : quat, || q || = 0 <-> q = qzero.
Proof.
intros. unfold qlen.
rewrite sqrt_eq0_iff. rewrite <- qlen2_eq0_iff. pose proof (qlen2_ge0 q). ra.
Qed.

|| q || <> 0 <-> q <> qzero

Lemma qlen_neq0_iff : forall q : quat, || q || <> 0 <-> q <> qzero.
Proof.
intros. unfold qlen.
rewrite sqrt_neq0_iff. rewrite <- qlen2_eq0_iff. pose proof (qlen2_ge0 q). ra.
Qed.

|| q1 || = || q2 || <-> qlen2 q1 = qlen2 q2

Lemma qlen_eq_iff_qlen2_eq : forall q1 q2 : quat,
    || q1 || = || q2 || <-> qlen2 q1 = qlen2 q2.
Proof.
  intros. rewrite <- !sqr_qlen. split; intros; ra.
  apply Rsqr_inj; auto. all: apply qlen_ge0.
Qed.

Unit quaternion

A unit quaternion has a magnitude equal to 1

Definition qunit (q : quat) : Prop := ||q || = 1.

vunit v -> qunit 0,v

Lemma im2q_qunit : forall v : vec 3, vunit v -> qunit (im2q v).
Proof.
intros. v2e v. cbv in H. cbv. ra. rewrite associative. rewrite H. ra.
Qed.

qunit q <-> qlen2 q = 1

Lemma qunit_iff_qlen2_eq1 : forall q : quat, qunit q <-> qlen2 q = 1.
Proof. intros. unfold qunit, qlen, qlen2 in *. split; intros; ra. Qed.

qunit q -> q.W ^ 2 = 1 - q.X ^ 2 - q.Y ^ 2 - q.Z ^ 2

Lemma qunit_imply_W : forall q : quat,
qunit q -> q.W ^ 2 = (1 - q.X ^ 2 - q.Y ^ 2 - q.Z ^ 2)%R.
Proof. intros. apply qunit_iff_qlen2_eq1 in H. rewrite <- H. cbv. ring. Qed.

qunit q -> q.W ^ 2 + q.X ^ 2 + q.Y ^ 2 + q.Z ^ 2 = 1

Lemma qunit_imply_Im : forall q : quat,
qunit q -> (q.W ^ 2 + q.X ^ 2 + q.Y ^ 2 + q.Z ^ 2 = 1)%R.
Proof. intros. apply qunit_iff_qlen2_eq1 in H. rewrite <- H. cbv. ring. Qed.

qunit q -> q <> qzero

Lemma qunit_neq0 : forall q : quat, qunit q -> q <> qzero.
Proof. intros. apply qlen_neq0_iff. intro. unfold qunit in H. lra. Qed.

q.W = 0 -> vunit (q.Im) -> qunit q

Lemma qim_vunit_imply_qunit : forall q : quat, q.W = 0 -> vunit (q.Im) -> qunit q.
Proof. intros. apply qunit_iff_qlen2_eq1. q2e q. cbv in *. ra. Qed.

Quaternion normalization

Quaternion addition 四元数加法

Definition qadd (q1 q2 : quat) : quat := vadd q1 q2.
Notation "p + q" := (qadd p q) : quat_scope.

Quaternion negation 四元数取负

Definition qopp (q : quat) : quat := vopp q.
Notation "- q" := (qopp q) : quat_scope.

Quaternion subtraction 四元数减法

Definition qsub (q1 q2 : quat) : quat := q1 + (-q2 ).
Notation "p - q" := (qsub p q) : quat_scope.

Quaternion multiplication

Quaternion multiplication, also known as "Hamilton product"

Definition qmul (q1 q2 : quat) : quat :=
  l2v [q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z ;
       q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y ;
       q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X ;
       q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W ]%R.
Notation "p * q" := (qmul p q) : quat_scope.

Multiplication of two quaternions by vector form，(p96) |pw| |qw| | pw qw - <pv,qv> | p * q = |pv| + |qv| = |pw qv + qw pv + pv × qv| This formula is also known Graβmann Product.

Lemma qmul_spec (p q : quat) :
  p * q =
    si2q
      (p.W * q.W - <p.Im, q.Im>)
      (p.W s* q.Im + q.W s* p.Im + p.Im \x q.Im)%V.
Proof. q2e p. q2e q. veq; ra. Qed.

qlen2 (q1 * q2) = (qlen2 q1) * (qlen2 q2)

Lemma qlen2_qmul : forall (q1 q2 : quat),
qlen2 (q1 * q2) = ((qlen2 q1 ) * (qlen2 q2 ))%R.
Proof. intros. q2e q1. q2e q2. cbv. ra. Qed.

|| q1 * q2 || = ||q1|| * ||q2||

Lemma qlen_qmul : forall (q1 q2 : quat), || q1 * q2 || = (||q1 || * ||q2 ||)%R.
Proof.
intros. apply Rsqr_inj. apply qlen_ge0. apply Rmult_le_pos; apply qlen_ge0.
autorewrite with R. rewrite !sqr_qlen. q2e q1. q2e q2. cbv. ra.
Qed.

qunit p -> qunit q -> qunit (p * q)

Lemma qmul_qunit (p q : quat) : qunit p -> qunit q -> qunit (p * q).
Proof.
intros. q2e p. q2e q. unfold qunit. rewrite qlen_qmul. rewrite H,H0. ring.
Qed.

(q * r) * m = q * (r * m)

Lemma qmul_assoc (q r m : quat) : (q * r ) * m = q * (r * m ).
Proof. q2e q. q2e r. q2e m. veq; ra. Qed.

The multiplication is non-commutative. That is: p * q <> q * p.

Lemma qmul_comm_fail : exists (p q : quat ), p * q <> q * p.
Proof. exists (l2v [0;1;2;1]). exists (l2v [0;2;1;2]). apply v4neq_iff. cbv. ra. Qed.

q * (r + m) = q * r + q * m

Lemma qmul_qadd_distr_l (q r m : quat) : q * (r + m ) = q * r + q * m.
Proof. q2e q. q2e r. q2e m. veq; ra. Qed.

(r + m) * q = r * q + m * q

Lemma qmul_qadd_distr_r (r m q : quat) : (r + m ) * q = r * q + m * q.
Proof. q2e q. q2e r. q2e m. veq; ra. Qed.

multplication of two pure quaternions: (0,u) * (0,v) = (-<u,v>, u × v)

Lemma qmul_im2q_eq (u v : vec 3) :
(im2q u ) * (im2q v ) = si2q (- <u,v>) (u \x v).
Proof. v2e u. v2e v. veq; ra. Qed.

(s1,0) * (s2,0) = (s1*s2,0)

Lemma qsqr_s2q : forall s1 s2 : R,
(s2q s1 ) * (s2q s2 ) = s2q (s1 * s2).
Proof. intros. veq; ra. Qed.

vunit u -> (0,u) * (0,u) = (-1,0)

Lemma qsqr_im2q : forall v : vec 3,
    vunit v -> (im2q v ) * (im2q v ) = s2q (-1).
Proof.
  intros. pose proof (v3unit_sqr_x v H).
  v2e v. cbv in H0. cbv. rewrite H0. veq; ra.
Qed.

Left scalar multiplication

Definition qscal (a : R) (q : quat) : quat := (a s* q)%V.
Notation "a s* q" := (qscal a q) : quat_scope.

1 s* q = q

Lemma qscal_1_l : forall q : quat, 1 s * q = q.
Proof. intros. q2e q. veq; ra. Qed.

(a s* p) * q = a s* (p * q)

Lemma qmul_qscal_l : forall (a : R) (p q : quat), (a s * p ) * q = a s * (p * q ).
Proof. intros. q2e p. q2e q. veq; ra. Qed.

p * (a s* q) = a s* (p * q)

Lemma qmul_qscal_r : forall (a : R) (p q : quat), p * (a s * q ) = a s * (p * q ).
Proof. intros. q2e p. q2e q. veq; ra. Qed.

a s* (b s* q) = (a * b) s* q

Lemma qscal_assoc : forall (a b : R) (q : quat), a s * (b s * q ) = (a * b ) s * q.
Proof. intros. q2e q. veq; ra. Qed.

qlen2 (a s* q) = a² * (qlen2 q)

Lemma qlen2_qscal : forall (a : R) (q : quat), qlen2 (a s * q) = (a ² * (qlen2 q ))%R.
Proof. intros. q2e q. cbv; ra. Qed.

a s* q = qzero <-> (a = 0) \/ (q = qzero)

Lemma qscal_eq0_iff : forall (q : quat) (a : R),
a s * q = qzero <-> (a = 0) \/ (q = qzero ).
Proof. intros. q2e q. rewrite !v4eq_iff. cbv. ra. Qed.

a s* q <> qzero <-> (a <> 0) /\ (q <> qzero)

Lemma qscal_neq0_iff : forall (q : quat) (a : R),
a s * q <> qzero <-> (a <> 0) /\ (q <> qzero ).
Proof. intros. pose proof(qscal_eq0_iff q a). tauto. Qed.

Right scalar multiplication

Definition qmulc (q : quat) (s : R) : quat := qscal s q.
Notation "q *s a" := (qmulc q a) : quat_scope.

s s* q = q *s s

Lemma qscal_eq_qmulc (s : R) (q : quat) : s s * q = q *s s.
Proof. auto. Qed.

The matrix form of quaternion multiplication

Quaternion matrix function, also be found in QQ,p96,p+

Definition qmatL (q : quat) : smat 4 :=
  let '(w,x,y,z) := (q.1,q.2,q.3,q.4) in
  l2m [[w; -x; -y; -z];
       [x; w; -z; y];
       [y; z; w; -x];
       [z; -y; x; w]]%R.

Verify the construction

Lemma qmatL_spec1 : forall q : quat,
    let m1 : smat 4 := (q.W s* mat1)%M in
    let m2a : vec 4 := vconsH 0 (-(q.Im))%V in
    let m2b : mat 3 4 := mconscH (q.Im) (skew3 (q.Im)) in
    let m2 : smat 4 := mconsrH m2a m2b in
    qmatL q = (m1 + m2)%M.
Proof. intros. unfold m1,m2,m2a,m2b. clear. q2e q. meq; ra. Qed.

p * q = L(p) * q

Lemma qmatL_spec2 : forall (p q : quat), p * q = (qmatL p) *v q.
Proof. intros. q2e p. q2e q. veq; ra. Qed.

Right matrix form of a quaternion, also be found in QQ,p96,p-

Definition qmatR (q : quat) : smat 4 :=
  let '(w,x,y,z) := (q.1,q.2,q.3,q.4) in
  l2m [[w; -x; -y; -z];
       [x; w; z; -y];
       [y; -z; w; x];
       [z; y; -x; w]]%R.

Verify the construction

Lemma qmatR_spec1 : forall q : quat,
    let m1 : smat 4 := (q.W s* mat1)%M in
    let m2a : vec 4 := vconsH 0 (-q.Im)%V in
    let m2b : mat 3 4 := mconscH (q.Im) (-(skew3 (q.Im)))%M in
    let m2 : smat 4 := mconsrH m2a m2b in
    qmatR q = (m1 + m2)%M.
Proof. intros. unfold m1,m2,m2a,m2b. clear. q2e q. meq; ra. Qed.

p * q = R(q) * p

Lemma qmatR_spec2 : forall (p q : quat), p * q = (qmatR q) *v p.
Proof. intros. q2e p. q2e q. veq; ra. Qed.

Identity quaternion 恒等四元数

恒定四元数：角位移为0的四元数（因为角位移就是朝向的变换，所以这里就是恒等元）

几何上有两个恒等四元数：(1,0̂) 和 (-1,0̂) 当 θ 是 2π 的偶数倍时，cos (θ/2) = 1, sin(θ/2) = 0, n̂是任意值当 θ 是 2π 的奇数倍时，cos (θ/2) = -1, sin(θ/2) = 0, n̂是任意值直观上，若旋转角度是绕任何轴转完整的整数圈，则在三维中方向上不会有任何实际的改变。

代数上只有一个恒等四元数 0̂ 1。因为要求任意 q 乘以单位元后不变。

Make a identity quaternion, i.e., (1,0,0,0)

Definition qone : quat := s2q 1.

(-1,0,0,0)

Definition qoneNeg : quat := s2q (-1).

ToDo: 是否可证只有 qone 是唯一的恒等四元数？

1 * q = q

Lemma qmul_1_l : forall q : quat, qone * q = q.
Proof. intros. q2e q. veq; ra. Qed.

q * 1 = q

Lemma qmul_1_r : forall q : quat, q * qone = q.
Proof. intros. q2e q. veq; ra. Qed.

Quaternion conjugate

当只使用单位四元数时，共轭和逆相等。 q和q∗ 代表相反的角位移。可直观的验证这种想法：q∗使得v变成了-v，所以旋转轴反向，颠倒了正方向，所以是相反的。

Conjugate of a quaternion

Definition qconj (q : quat) : quat := si2q (q.W) (- q.Im)%V.

Notation "q \*" := (qconj q) (at level 30) : quat_scope.

q \* \* = q

Lemma qconj_qconj (q : quat) : q \* \* = q.
Proof. q2e q. veq; ra. Qed.

(im2q v)\* = im2q (-v)

Lemma qconj_im2q : forall (v : vec 3), (im2q v )\* = im2q (-v)%V.
Proof. intros. v2e v. veq; ra. Qed.

(p * q)\* = q\* * p\*

Lemma qconj_qmul (p q : quat) : (p * q )\* = q \* * p \*.
Proof. intros. q2e p. q2e q. veq; ra. Qed.

(a s* q)\* = a s* (q\* )

Lemma qconj_qscal : forall (a : R) (q : quat), (a s * q )\* = a s * (q \*).
Proof. intros. q2e q. veq; ra. Qed.

(p + q)\* = p\* + q\*

Lemma qconj_qadd (p q : quat) : (p + q )\* = p \* + q \*.
Proof. intros. q2e p. q2e q. veq; ra. Qed.

q * q\* = q\* * q

Lemma qmul_qconj_comm (q : quat) : q * q \* = q \* * q.
Proof. intros. q2e q. veq; ra. Qed.

Im (q\* * q) = 0

Lemma qmul_qconj_l_Im0 (q : quat) : (q \* * q ).Im = vzero.
Proof. intros. q2e q. veq; ra. Qed.

Im (q * q\* ) = 0

Lemma qmul_qconj_r_Im0 (q : quat) : (q * q \*).Im = vzero.
Proof. intros. q2e q. veq; ra. Qed.

qunit q -> q\* * q = qone

Lemma qunit_qmul_qconj_l : forall q, qunit q -> q \* * q = qone.
Proof. intros. q2e q. veq; ra. Qed.

qunit q -> q * q\* = qone

Lemma qunit_qmul_qconj_r : forall q, qunit q -> q * q \* = qone.
Proof. intros. q2e q. veq; ra. Qed.

|| q\* || = || q ||

Lemma qlen_qconj (q : quat) : || q \* || = || q ||.
Proof.
intros. q2e q. cbv; ra.
Qed.

|| q\* * q || = qlen2 q

Lemma qlen_qmul_qconj_l : forall (q : quat), || q \* * q || = qlen2 q.
Proof. intros. rewrite qlen_qmul. rewrite qlen_qconj. apply sqr_qlen. Qed.

|| q * q\* || = qlen2 q

Lemma qlen_qmul_qconj_r : forall (q : quat), || q * q \* || = qlen2 q.
Proof. intros. rewrite qlen_qmul. rewrite qlen_qconj. apply sqr_qlen. Qed.

qunit q <-> qunit (q\* )

Lemma qconj_qunit : forall (q : quat), qunit (q \*) <-> qunit q.
Proof. intros. unfold qunit. rewrite qlen_qconj. easy. Qed.

L(q\* ) = L(q)\T

Lemma qmatL_qconj : forall q : quat, qmatL (q \*) = (qmatL q)\T.
Proof. intros. q2e q. meq; ra. Qed.

R(q\* ) = R(q)\T

Lemma qmatR_qconj : forall q : quat, qmatR (q \*) = (qmatR q)\T.
Proof. intros. q2e q. meq; ra. Qed.

L(q) R(q\* )

Definition qmat (q : quat) : smat 4 :=
  let '(w,x,y,z) := (q.1,q.2,q.3,q.4) in
  l2m [[1; 0; 0; 0];
       [0; 1 - 2*y² - 2*z² ; 2*x*y - 2*w*z; 2*w*y + 2*x*z];
       [0; 2*x*y + 2*w*z; 1 - 2*x² - 2*z² ; 2*y*z - 2*w*x];
       [0; 2*x*z - 2*w*y; 2*w*x + 2*y*z; 1 - 2*x² - 2*y² ]]%R.

qunit q -> q v q* = L(q) R(q* ) v

Lemma qmat_spec : forall (q v : quat),
    qunit q -> q * v * q \* = (qmat q) *v v.
Proof.
  intros. q2e q. q2e v. apply qunit_imply_W in H.
  cbv in H. ring_simplify in H. veq; field_simplify; rewrite H; lra.
Qed.

Quaternion inverse

inversion of quaternion

Definition qinv (q : quat) : quat := (/ (qlen2 q )) s * (q \*).
Notation "q \-1" := (qinv q) : quat_scope.

q \-1 * q = 1

Lemma qmul_qinv_l : forall q : quat, q <> qzero -> q \-1 * q = qone.
Proof. intros. q2e q. apply v4neq_iff in H. cbv in H. veq; ra. Qed.

q * q \-1 = 1

Lemma qmul_qinv_r : forall q : quat, q <> qzero -> q * q \-1 = qone.
Proof. intros. q2e q. apply v4neq_iff in H. cbv in H. veq; ra. Qed.

qunit q -> q \-1 = q \*

Lemma qinv_eq_qconj : forall q : quat, qunit q -> q \-1 = q \*.
Proof.
intros. unfold qinv. apply qunit_iff_qlen2_eq1 in H. rewrite H.
autorewrite with R. rewrite qscal_1_l. auto.
Qed.

(p * q)\-1 = q\-1 * p\-1

Lemma qinv_qmul : forall p q : quat, p <> qzero -> q <> qzero -> (p * q )\-1 = q \-1 * p \-1.
Proof.
  intros. unfold qinv. rewrite qconj_qmul.
  rewrite qmul_qscal_l. rewrite qmul_qscal_r. rewrite qscal_assoc. f_equal.
  rewrite qlen2_qmul. field. split; apply qlen2_neq0_iff; auto.
Qed.

(a s* q)\-1 = (1/a) s* q\-1

Lemma qinv_qscal : forall (q : quat) (a : R),
    a <> 0 -> q <> qzero -> (a s * q )\-1 = (1/a ) s * q \-1.
Proof.
  intros.
  unfold qinv. rewrite qlen2_qscal.
  rewrite qconj_qscal. rewrite !qscal_assoc. f_equal.
  unfold Rsqr. field. apply qlen2_neq0_iff in H0. auto.
Qed.

p * q = r -> p = r * q\-1

Lemma qmul_imply_solve_l : forall p q r : quat, q <> qzero -> p * q = r -> p = r * q \-1.
Proof.
intros. rewrite <- H0. rewrite qmul_assoc, qmul_qinv_r, qmul_1_r; auto.
Qed.

p * q = r -> q = p\-1 * r

Lemma qmul_imply_solve_r : forall p q r : quat, p <> qzero -> p * q = r -> q = p \-1 * r.
Proof.
intros. rewrite <- H0. rewrite <- qmul_assoc, qmul_qinv_l, qmul_1_l; auto.
Qed.

以下几个公式是我发现的，或许本质上很简单

L(q) * R(q\-1) = R(q\-1) * L(q)

Lemma qmul_qL_qR_qinv_comm : forall q,
    let m1 := qmatL q in
    let m2 := qmatR (q \-1) in
    (m1 * m2 = m2 * m1)%M.
Proof. intros. unfold m1,m2. clear. q2e q. meq; ra. Qed.

L(q\-1) * L(q)\T = L(q)\T * L(q\-1)

Lemma qmul_qL_qinv_qtrans_qL_comm : forall q,
    let m1 := qmatL (q \-1) in
    let m2 := (qmatL q)\T in
    (m1 * m2 = m2 * m1)%M.
Proof. intros. unfold m1,m2. clear. q2e q. meq; ra. Qed.

R(q\-1) * L(q)\T = L(q)\T * R(q\-1)

Lemma qmul_qR_qinv_qtrans_qL_comm : forall q,
    let m1 := qmatR (q \-1) in
    let m2 := (qmatL q)\T in
    (m1 * m2 = m2 * m1)%M.
Proof. intros. unfold m1,m2. clear. q2e q. meq; ra. Qed.

L(q\-1) * R(q\-1) = R(q\-1) * L(q\-1)

Lemma qmul_qL_qinv_qR_qinv_comm : forall q,
    let m1 := qmatL (q \-1) in
    let m2 := qmatR (q \-1) in
    (m1 * m2 = m2 * m1)%M.
Proof. intros. unfold m1,m2. clear. q2e q. meq; ra. Qed.

Divide of quaternion

利用除法，可以计算两个给定旋转 a 和 b 之间的某种角位移 d。在 Slerp 时会使用它。

If r * p = q, then r ≜ q * p\-1 表示从p到q旋转的角位移

Definition qdiv (q p : quat) : quat := p * q \-1.

Lemma qdiv_spec : forall a b : quat, a <> qzero -> (qdiv a b ) * a = b.
Proof. intros. unfold qdiv. rewrite qmul_assoc,qmul_qinv_l,qmul_1_r; auto. Qed.

Unit quaternion <-> Axis-Angle

Unit quaternion and the Axis-Angle representation are bijection. That is, every unit quaternion has a corresponded unique axis-angle value, every axis-angle value has a corresponded unique unit quaternion.

Convert axis-angle value to unit quaternion

Definition aa2quat (aa : AxisAngle) : quat :=
let (n,θ) := aa in
si2q (cos (θ /2)) (sin (θ /2) s* n)%V.

Any quaternion constructed from axis-angle is unit quaternion

Lemma aa2quat_unit : forall aa : AxisAngle,
    let (n,θ) := aa in
    vunit n -> qunit (aa2quat aa).
Proof.
  intros. destruct aa as [n θ]. intros.
  pose proof (v3unit_sqr_x n H).
  v2e n. cbv. cbv in H0. ra. rewrite H0.
  rewrite sqrt_eq1_if_eq1; auto. ring_simplify. ra.
Qed.

Convert unit quaternion to axis-angle value

Definition quat2aa (q : quat) : AxisAngle :=
  let θ : R := (2 * acos (q.W))%R in
  let n : vec 3 := ((1 / sqrt (1-q.W ²)) s* q.Im)%V in
  mkAA n θ.

若q = aa(θ,n) = (cos(θ/2), sin(θ/2)*n) 是单位向量，则： (1) 当 q = (1,0,0,0)，则θ为2kπ；否则 θ≠2kπ 且 n 是单位向量。 (2) 当 q ≠ (1,0,0,0)，则n一定是单位向量

Lemma quat2aa_unit : forall (q : quat) (H : qunit q) (H1 : q <> qone),
    let (n,θ) := quat2aa q in vunit n.
Proof.
  intros.
  pose proof (qunit_imply_W q H).
  q2e q. simpl in *. apply v4neq_iff in H1. cbv in *. ra.
  rewrite Rsqr_inv'. rewrite Rsqr_sqrt; ra. field_simplify_eq; ra.
Abort.

Lemma aa2quat_quat2aa_id : forall q : quat, aa2quat (quat2aa q) = q.
Proof.
  intros. q2e q. veq; ra.
Abort.

Lemma quat2aa_aa2quat_id : forall aa : AxisAngle, quat2aa (aa2quat aa) = aa.
Proof.
  intros. destruct aa. cbv. f_equal; ra.
Abort.

Rotate 3D vector by unit quaternion

vector v (be wrapped to quaterion) is rotated by a quaternion q. Note that: q must be a rotation quaternion

Definition qrot (q : quat) (v : quat) : quat := q * v * q \*.

vector form of qrot

Definition qrotv (q : quat) (v : vec 3) : vec 3 := q2im (qrot q (im2q v)).

证明四元数乘法能够旋转三维矢量

方法1：计算其生成的结果与轴角表示的结果相同，这是大多数文献所采用的方法。

Lemma qrot_spec1 : forall (aa : AxisAngle) (v : vec 3),
    vunit (aaAxis aa) ->
    let q := aa2quat aa in
    qrotv q v = rotaa aa v.
Proof.
  intros. destruct aa as [n θ].
  pose proof (v3unit_sqr_x n H).
  v2e n. v2e v. simpl in *.
  unfold q. rewrite Ha in *. cbv in H, H0. veq.
  - field_simplify; ra; rewrite H0, cos2; try lra.
    remember (θ / (R1 + R1 ))%R as α. replace θ with (α + α)%R by lra.
    rewrite cos_plus, sin_plus. unfold Rminus. field_simplify; try lra.
    simp_pow. field_simplify. rewrite cos2. field_simplify; lra.
  - field_simplify; ra; rewrite H0, cos2; try lra.
    remember (θ / (R1 + R1 ))%R as α. replace θ with (α + α)%R by lra.
    rewrite cos_plus, sin_plus. unfold Rminus. field_simplify; try lra.
    simp_pow. field_simplify. rewrite cos2. field_simplify; lra.
  - field_simplify; ra; rewrite H0, cos2; try lra.
    remember (θ / (R1 + R1 ))%R as α. replace θ with (α + α)%R by lra.
    rewrite cos_plus, sin_plus. unfold Rminus. field_simplify; try lra.
    simp_pow. field_simplify. rewrite cos2. field_simplify; lra.
Qed.

方法2：QQ书上的推导过程

qrot对向量加法是线性的

Lemma qrot_linear_vadd : forall (q : quat) (v1 v2 : vec 3),
(qrotv q (v1 + v2) = qrotv q v1 + qrotv q v2)%V.
Proof. intros. veq; ra. Qed.

qrot对向量数乘是线性的

Lemma qrot_linear_vscal : forall (q : quat) (v : vec 3) (k : R),
(qrotv q (k s* v) = k s* (qrotv q v))%V.
Proof. intros. veq; ra. Qed.

qrot作用于某个四元数后不改变它的w分量。公式5.26

Lemma qrot_keep_w : forall (q v : quat),
    qunit q -> (qrot q v ).W = v.W.
Proof.
  intros. pose proof (qunit_imply_W q H). q2e q. q2e v. cbv in *. ra.
  field_simplify. ra. rewrite H0. ra.
Qed.

qrot作用于某个纯四元数后所得四元数的w分量为0，即仍然是纯四元数

Lemma qrot_im2q_w0 : forall (q : quat) (v : vec 3),
qunit q -> (qrot q (im2q v)).W = 0.
Proof. intros. rewrite qrot_keep_w; auto. Qed.

单位四元数的另一种表示形式：由旋转轴(单位向量)和旋转角构成 5.25

qrot operation keep vector dot product

Lemma qrot_keep_dot : forall (q : quat) (v1 v2 : vec 3),
    qunit q -> < qrotv q v1, qrotv q v2> = <v1, v2>.
Proof.
  intros. pose proof (qunit_imply_W q H).
  v2e v1. v2e v2. q2e q; cbv in *.
  field_simplify. ra. rewrite H0. ra.
Qed.

qrot operation and vnorm operation are commutative

Lemma qrot_vnorm_comm : forall (q : quat) (v : vec 3),
    qunit q -> vnorm (qrotv q v) = qrotv q (vnorm v).
Proof.
  intros. unfold vnorm. unfold vlen, Vector.vlen.
  rewrite qrot_keep_dot; auto. rewrite qrot_linear_vscal. easy.
Qed.

qrot operation keep vector length

Lemma qrot_keep_vlen : forall (q : quat) (v : vec 3),
qunit q -> (|| qrotv q v || = || v ||)%V.
Proof. intros. unfold vlen, Vector.vlen. f_equal. rewrite qrot_keep_dot; auto. Qed.

qrot operation keep vector angle

Lemma qrot_keep_vangle : forall (q : quat) (v1 v2 : vec 3),
    qunit q -> v1 /_ v2 = (qrotv q v1) /_ (qrotv q v2).
Proof.
  intros. unfold vangle. f_equal.
  rewrite !qrot_vnorm_comm; auto. rewrite qrot_keep_dot; auto.
Qed.

qrot operation keep unit vector

Lemma qrot_im2q_vunit : forall (q : quat) (v : vec 3),
    qunit q -> vunit v -> vunit (qrotv q v).
Proof.
  intros. apply vunit_spec. rewrite qrot_keep_vlen; auto.
  apply vunit_spec; auto.
Qed.

qrot operation with unit vector yields unit quaternion

Lemma qrot_im2q_qunit : forall (q : quat) (v : vec 3),
    qunit q -> vunit v -> qunit (qrot q (im2q v)).
Proof.
  intros. apply qim_vunit_imply_qunit; auto.
  apply qrot_im2q_w0; auto. apply qrot_im2q_vunit; auto.
Qed.

Definition ab2q (u v : vec 3) : quat := si2q (<u,v>) (u \x v).

Definition ab2q' (u v : vec 3) : quat := (im2q v ) * ((im2q u )\* ).

两种方式定义出的四元数相等，(0,v) ⊗ (0,u)\* = (<u,v>,u \x v)

Lemma ab2q_eq_ab2q' : forall u v : vec 3, ab2q u v = ab2q' u v.
Proof. intros. v2e u. v2e v. veq; ra. Qed.

该四元数是单位四元数 vunit u -> vunit v -> qunit (ab2q u v)

Lemma ab2q_qunit : forall u v : vec 3,
    vunit u -> vunit v -> qunit (ab2q u v).
Proof.
  intros. rewrite ab2q_eq_ab2q'. unfold ab2q'. apply qmul_qunit.
  apply im2q_qunit; auto.
  rewrite qconj_qunit. apply im2q_qunit; auto.
Qed.

任给两个单位向量v0,v1，并由它们的夹角和垂直轴确定出一个四元数q，若将v1由q 旋转后得到v2，则(v1,v2)所确定的四元数也等于q q

Lemma ab2q_eq : forall (v0 v1 : vec 3),
    let q : quat := ab2q v0 v1 in
    let v2 : vec 3 := qrotv q v0 in
    vunit v0 -> vunit v1 -> ab2q v1 v2 = q.
Proof.
  intros.
  rewrite ab2q_eq_ab2q'. unfold ab2q'. unfold v2. unfold qrotv.
  rewrite im2q_q2im.
  2:{ rewrite qrot_im2q_w0; auto. apply ab2q_qunit; auto. }
  unfold qrot. unfold q at 2.
  rewrite ab2q_eq_ab2q'. unfold ab2q'.
  rewrite qconj_qmul, !qconj_qconj.
  rewrite <- qmul_assoc. rewrite (qmul_assoc q _ _). rewrite qsqr_im2q; auto.
  rewrite qmul_assoc. rewrite qconj_im2q. rewrite qsqr_im2q.
  rewrite qmul_assoc. rewrite qsqr_s2q. q2e q; veq; ra.
  rewrite vopp_vunit; auto.
Qed.

qrot保持向量的单位性

Lemma qrot_keep_vunit : forall (q : quat) (v : vec 3),
qunit q -> vunit v -> qunit (qrot q (im2q v)).
Proof. intros. apply qrot_im2q_qunit; auto. Qed.

论证旋转，需要构造一些中间变量，所以逻辑有点绕

Section rotation_derivation.
  Variables (θ : R) (n v0 v1 : vec 3).
  Hypotheses (Hbound_θ : 0 < θ < 2 * PI)
    (Hunit_v0: vunit v0) (Hunit_v1: vunit v1)
    (Hnorm_v01_n: vnorm (v0 \x v1) = n)
    (Hangle_v01_θ: vangle v0 v1 = θ /2).

  Let q : quat := aa2quat (mkAA n θ).

一组关于 θ 的断言，暂时未使用

Section about_θ.

0 < sin (θ/2)

Fact sin_θ2_gt0 : 0 < sin (θ /2).
Proof. rewrite <- Hangle_v01_θ. apply sin_gt_0; ra. Qed.

θ = 0 <-> sin (θ/2) = 0

    Fact θ_eq0_iff_sin_θ2_eq0 : θ = 0 <-> sin (θ /2) = 0.
    Proof.
      split; intros H.
      - apply sin_eq_O_2PI_1; ra.
      - apply sin_eq_O_2PI_0 in H; ra.
    Qed.

θ ≠ 0 <-> sin (θ/2) ≠ 0

Fact θ_neq0_iff_sin_θ2_neq0 : θ <> 0 <-> sin (θ /2) <> 0.
Proof. rewrite θ_eq0_iff_sin_θ2_eq0. easy. Qed.

θ = 0 <-> ||v0\xv1|| = 0

    Fact θ_eq0_iff_v01_vcross_len0 : θ = 0 <-> (||v0 \x v1|| = 0)%V.
    Proof.
      rewrite vlen_v3cross_eq0_iff_angle_0_pi; auto; ra.
      all: apply vunit_neq0; auto.
    Qed.

θ = 0 <-> v0 \x v1 = vzero

    Fact θ_eq0_iff_v01_vcross_eq0 : θ = 0 <-> v0 \x v1 = vzero.
    Proof.
      rewrite <- (vlen_eq0_iff_eq0 (v0 \x v1)).
      rewrite θ_eq0_iff_v01_vcross_len0. easy.
    Qed.
  End about_θ.

1. 基本的事实

v0 _| n

  Fact v0_orth_n : v0 _|_ n.
  Proof.
    rewrite <- Hnorm_v01_n. rewrite vorth_vnorm_r.
    - apply vorth_comm. apply v3cross_orth_l.
    - intro. apply θ_eq0_iff_v01_vcross_eq0 in H. ra.
  Qed.

v1 _| n

  Fact v1_orth_n : v1 _|_ n.
  Proof.
    rewrite <- Hnorm_v01_n. apply vorth_vnorm_r.
    - intro. apply θ_eq0_iff_v01_vcross_eq0 in H. ra.
    - apply vorth_comm. apply v3cross_orth_r.
  Qed.

n is unit

  Fact Hunit_n : vunit n.
  Proof.
    rewrite <- Hnorm_v01_n. apply vnorm_is_unit.
    intro. apply θ_eq0_iff_v01_vcross_eq0 in H. ra.
  Qed.

(<v0,v1>, v0 \x v1) = q

  Fact v01_eq_q : ab2q v0 v1 = q.
  Proof.
    unfold q. unfold aa2quat. unfold ab2q. f_equal.
    - rewrite vdot_eq_cos_angle. rewrite <- Hangle_v01_θ.
      rewrite !vunit_imply_vlen_eq1; auto. ra.
    - rewrite v3cross_eq_vscal; ra.
      rewrite Hangle_v01_θ, Hnorm_v01_n.
      rewrite !vunit_imply_vlen_eq1; auto. ra.
      all: apply vunit_neq0; auto.
  Qed.

q 是单位向量

Fact q_qunit : qunit q.
Proof. rewrite <- v01_eq_q. apply ab2q_qunit; auto. Qed.

2. 证明 (v1,v2) 与 (v0,v1) 的几何关系

Let v2 : vec 3 := qrotv q v0.

v2 是单位向量，即长度不变

Fact v2_vunit : vunit v2.
Proof. unfold v2. apply qrot_im2q_vunit; auto. apply q_qunit. Qed.

由 v1,v2 构造的四元数等于 q

  Fact v12_eq_q : ab2q v1 v2 = q.
  Proof.
    pose proof (ab2q_eq v0 v1 Hunit_v0 Hunit_v1) as H; simpl in H.
    rewrite v01_eq_q in H. auto.
  Qed.

<v1,v2> = <v0,v1>，保持点积

  Fact v12_v01_keep_vdot : <v1,v2> = <v0,v1>.
  Proof.
    pose proof (v12_eq_q). rewrite <- v01_eq_q in H. unfold ab2q in H.
    apply si2q_inj in H; destruct H; auto.
  Qed.

v1 \x v2 = v0 \x v1, 表明(v1,v2)和(v0,v1)所确定的垂直轴相同

  Fact v12_v01_keep_vcross : v1 \x v2 = v0 \x v1.
  Proof.
    pose proof (v12_eq_q). rewrite <- v01_eq_q in H. unfold ab2q in H.
    apply si2q_inj in H; destruct H; auto.
  Qed.

v2 _| n

  Fact v2_orth_n : v2 _|_ n.
  Proof.
    copy Hnorm_v01_n.
    rewrite <- v12_v01_keep_vcross in HC. rewrite <- HC.
    apply vorth_vnorm_r.
    - intro. rewrite v12_v01_keep_vcross in H.
      apply θ_eq0_iff_v01_vcross_eq0 in H. ra.
    - apply vorth_comm. apply v3cross_orth_r.
  Qed.

(v1,v2)和(v0,v1)的这两个夹角相同

  Fact v12_v01_same_angle : v1 /_ v2 = v0 /_ v1.
  Proof.
    unfold vangle. f_equal.
    rewrite !vdot_vnorm. rewrite !vunit_imply_vlen_eq1; auto.
    rewrite v12_v01_keep_vdot. auto.
    all: try apply vunit_neq0; auto; try apply v2_vunit.
  Qed.

给定两个向量，若将这两个向量同时旋转θ角，则向量之和在旋转前后的夹角也是θ。

  Lemma vangle_vadd : forall {n} (a b c d : vec n),
      a <> vzero -> b <> vzero ->
      ||a||%V = ||c||%V -> ||b||%V = ||d||%V ->
      a /_ b = c /_ d ->
      ((a + b) /_ (c + d) = b /_ d)%V.
  Proof.
  Admitted.

  Lemma vangle_add : forall {n} (a b c : vec n), a /_ c = ((a /_ b ) + (b /_ c ))%R.
  Proof.
  Admitted.

(v0,v2) 的夹角是 θ

  Fact v02_angle_θ : v0 /_ v2 = θ.
  Proof.
    rewrite (vangle_add v0 v1 v2); ra.
    rewrite v12_v01_same_angle. rewrite Hangle_v01_θ. lra.
  Qed.

3. 证明 (v2,v3) 与 (v1,v2) 的几何关系

Let v3 : vec 3 := qrotv q v1.

v3 是单位向量，即长度不变

Fact v3_vunit : vunit v3.
Proof. unfold v3. apply qrot_im2q_vunit; auto. apply q_qunit. Qed.

由 v2,v3 构造的四元数等于 q

  Fact v23_eq_q : ab2q v2 v3 = q.
  Proof.
    pose proof (ab2q_eq v1 v2 Hunit_v1 v2_vunit) as H; simpl in H.
    rewrite v12_eq_q in H. auto.
  Qed.

<v2,v3> = <v1,v2>，保持点积

  Fact v23_v12_keep_vdot : <v2,v3> = <v1,v2>.
  Proof.
    intros. pose proof (v23_eq_q). rewrite <- v12_eq_q in H.
    apply si2q_inj in H; destruct H; auto.
  Qed.

v2 \x v3 = v1 \x v2, 表明(v2,v3)和(v1,v2)所确定的垂直轴相同

  Fact v23_v12_keep_vcross : v2 \x v3 = v1 \x v2.
  Proof.
    pose proof (v23_eq_q). rewrite <- v12_eq_q in H. unfold ab2q in H.
    apply si2q_inj in H; destruct H; auto.
  Qed.

v3 _| n

  Fact v3_orth_n : v3 _|_ n.
  Proof.
    assert (v0 \x v1 = v2 \x v3) as H.
    { rewrite v23_v12_keep_vcross, v12_v01_keep_vcross. easy. }
    copy Hnorm_v01_n.
    rewrite H in HC. rewrite <- HC. apply vorth_vnorm_r.
    - rewrite <- H. intro. apply θ_eq0_iff_v01_vcross_eq0 in H0. ra.
    - apply vorth_comm. apply v3cross_orth_r.
  Qed.

(v2,v3)和(v1,v2)的这两个夹角相同

  Fact v23_v12_same_angle : v2 /_ v3 = v1 /_ v2.
  Proof.
    unfold vangle. f_equal.
    rewrite !vdot_vnorm. rewrite !vunit_imply_vlen_eq1.
    rewrite v23_v12_keep_vdot. auto.
    all: try apply vunit_neq0; auto.
    all: try apply v2_vunit; try apply v3_vunit.
  Qed.

(v1,v3) 的夹角是 θ

  Fact v13_angle_θ : v1 /_ v3 = θ.
  Proof.
    rewrite (vangle_add v1 v2 v3); ra.
    rewrite v23_v12_same_angle, v12_v01_same_angle; lra.
  Qed.

v1,v2,v3 共面

  Fact v123_coplanar : v3coplanar v1 v2 v3.
  Proof.
    apply v3cross_same_v3coplanar. symmetry. apply v23_v12_keep_vcross.
  Qed.

4. 证明 q 不改变转轴 n

q ⊗ 0,n = 0,n ⊗ q

  Fact qn_eq_nq : q * im2q n = im2q n * q.
  Proof.
    unfold q. q2e n. veq; ra.
  Qed.

使用 q 对 n 旋转，不改变 n。即，符合几何上的直观含义：n 是旋转轴。

  Fact rot_n_eq_n : qrotv q n = n.
  Proof.
    unfold qrotv, qrot. rewrite qn_eq_nq. rewrite qmul_assoc.
    rewrite <- qinv_eq_qconj. rewrite qmul_qinv_r. rewrite qmul_1_r.
    apply q2im_im2q. apply qunit_neq0. apply q_qunit. apply q_qunit.
  Qed.

5. 证明 q 与任意向量 v 的几何关系

  Section main_theorem_analysis.

    Variable s0 s1 s2 : R.
    Hypotheses (Hs0_gt0 : 0 < s0) (Hs1_gt0 : 0 < s1).
    Let v : vec 3 := (s0 s* v0 + s1 s* v1 + s2 s* n)%V.
    Let v' : vec 3 := qrotv q v.

我们证明如下结论： (1) v和v'的长度相等 (2) v和v'在n的法平面上的投影的夹角是θ 这说明了 qrot 将 v 绕 n 轴旋转了 θ 度，到达 v'。所以，单位四元数旋转公式 0 v' = qrot (q, v) = q ⊗ 0 n ⊗ q\* , 其中，q = (cos(θ/2), sin(θ/2)*n) 表达式了我们想要的旋转。

v和v'的长度相等

    Fact vlen_vv' : (||v'|| = ||v||)%V.
    Proof.
      unfold v',v. rewrite qrot_keep_vlen; auto. apply q_qunit.
    Qed.

v和v'在n的法平面上的投影的夹角是θ

    Fact vangle_vv' : vperp v n /_ vperp v' n = θ.
    Proof.
      pose proof (vunit_neq0 n Hunit_n).
      unfold v',v.
      rewrite !qrot_linear_vadd, !qrot_linear_vscal.
      fold v2. fold v3. rewrite rot_n_eq_n.
      rewrite !vperp_vadd, !vperp_vscal; auto.
      rewrite vperp_self; auto. rewrite vscal_0_r, !vadd_0_r.
      rewrite !vorth_imply_vperp_eq_l.
      rewrite vangle_vadd.
      all: rewrite ?vangle_vscal_l_gt0, ?vangle_vscal_r_gt0; auto.
      all: try (apply vunit_neq0; try apply v3_vunit; try apply v2_vunit; easy).
      all: try apply v3_orth_n; try apply v2_orth_n; try apply v1_orth_n.
      all: try apply v0_orth_n; try apply v13_angle_θ.
      all: try apply vscal_neq0_neq0_neq0; unfold Azero; try lra.
      all: try (apply vunit_neq0; try apply v3_vunit; easy).
      all: rewrite ?vlen_vscal,?vunit_imply_vlen_eq1; auto.
      apply v2_vunit. apply v3_vunit.
      rewrite v23_v12_same_angle. rewrite v12_v01_same_angle. auto.
    Qed.
  End main_theorem_analysis.

End rotation_derivation.

四元数乘法能表示旋转（这一版，仍然使用 v0,v1 这两个中间变量，以后也许能去掉）

Theorem qrot_valid : forall (v0 v1 : vec 3) (s0 s1 s2 : R) (aa : AxisAngle),
    let (n, θ) := aa in
    let q : quat := aa2quat aa in
    let v : vec 3 := (s0 s* v0 + s1 s* v1 + s2 s* n)%V in
    let v' : vec 3 := qrotv q v in
    vunit v0 -> vunit v1 ->
    0 < θ < 2 * PI ->
    vnorm (v0 \x v1) = n ->
    vangle v0 v1 = θ /2 ->
    0 < s0 -> 0 < s1 ->
    (||v'|| = ||v||)%V /\ vperp v n /_ vperp v' n = θ.
Proof.
  intros. destruct aa as [θ n]. intros.
  split; [apply vlen_vv'|apply vangle_vv']; auto.
Qed.

方法3：Dunn 中提到的

Section qrot_spec_method3.
End qrot_spec_method3.

备注：四元数乘法可以连接多个旋转，就像矩阵乘法一样。但实际上还有一些区别。矩阵乘法时，可以使用行矢量左乘矩阵，也可使用列矢量右乘矩阵（转置形式）。而四元数没有这种灵活性，多个连接总是从右到左或说“从里到外” 读取。对于 Dunn 的这个观点，我们可以进一步实验，看看四元数是否也能通过某种 “转置”操作实现更换方向。当然，这个实验可能只是理论上的需要，实际并无此需求。由于四元数乘法有对应的矩阵形式，我么可以直接在矩阵上操作

Rotate twice: first by q1, then by q2

Lemma qrot_twice : forall (q1 q2 : quat) (v : quat),
qrot q2 (qrot q1 v) = qrot (q2 * q1) v.
Proof.
intros. unfold qrot. rewrite qconj_qmul, !qmul_assoc; auto.
Qed.

vector form

Lemma qrot_twice_vec : forall (q1 q2 : quat) (v : vec 3),
    qunit q1 -> qrotv q2 (qrotv q1 v) = qrotv (q2 * q1) v.
Proof.
  intros. unfold qrotv.
  rewrite <- qrot_twice; try apply qunit_neq0; auto.
  unfold qrot. f_equal. f_equal. f_equal. rewrite im2q_q2im; auto.
  pose proof (qrot_im2q_w0 q1 v H). unfold qrot in H0. auto.
Qed.

坐标系旋转的情况

Section qrotAxis.

向量v(被包装为四元数)所在的坐标系旋转q，则新坐标系中v的向量如下。注意，q必须是旋转四元数

  Definition qrotAxis (q : quat) (v : quat) : quat := q \* * v * q.

  Lemma qrotAxis_qrot : forall q v, qunit q -> qrot q (qrotAxis q v) = v.
  Proof.
    intros. unfold qrot, qrotAxis.
    rewrite <- !qmul_assoc. rewrite qunit_qmul_qconj_r; auto. rewrite qmul_1_l.
    rewrite !qmul_assoc. rewrite qunit_qmul_qconj_r; auto. rewrite qmul_1_r. auto.
  Qed.

  Lemma qrot_qrotAxis : forall q v, qunit q -> qrotAxis q (qrot q v) = v.
  Proof.
    intros. unfold qrot, qrotAxis.
    rewrite <- !qmul_assoc. rewrite qunit_qmul_qconj_l; auto. rewrite qmul_1_l.
    rewrite !qmul_assoc. rewrite qunit_qmul_qconj_l; auto. rewrite qmul_1_r. auto.
  Qed.

坐标系旋转q时，某向量在旧坐标系下的坐标 v 在新坐标系下为

  Definition qrotvAxis (q : quat) (v : vec 3) : vec 3 := q2im (qrotAxis q (im2q v)).

  Lemma qrotvAxis_eq_qrotv : forall q v, qrotvAxis q v = q2im (qrotAxis q (im2q v)).
  Proof. auto. Qed.

坐标系相继旋转q1和q2时，向量在就坐标系下的坐标v在新坐标系下为

  Lemma qrotAxis_twice : forall q1 q2 v,
      qrotAxis q2 (qrotAxis q1 v) = qrotAxis (q1 * q2) v.
  Proof.
    intros. unfold qrotAxis. rewrite qconj_qmul, !qmul_assoc; auto.
  Qed.

End qrotAxis.

Unit quaternions and rotation matrices 单位四元数和旋转矩阵

Convert a quaternion q_e2b (from {e} to {b}) to rotation matrix R_b2e ({b} to {e}) 将{e}到{b}的旋转四元数q_e2b 转换为{b}相对于{e}的旋转矩阵 R_b2e

Definition q2m (q : quat) : smat 3 :=
  let '(w,x,y,z) := (q.1,q.2,q.3,q.4) in
  l2m [[w^2+x^2-y^2-z^2; 2*x*y-2*w*z; 2*x*z+2*w*y];
       [2*x*y+2*w*z; w^2-x^2+y^2-z^2; 2*y*z-2*w*x];
       [2*x*z-2*w*y; 2*y*z+2*w*x; w^2-x^2-y^2+z^2]]%R.

1 0 quint q -> 0 C = qmatL (q\-1) * qmatR q, where C = q2m q

Lemma q2m_eq : forall q,
    let C := q2m q in
    qunit q ->
    mconsrH (vconsH 1 vzero) (mconscH vzero C) = (qmatL q * qmatR (q \-1))%M.
Proof.
  intros. rewrite qinv_eq_qconj; auto.
  unfold C. q2e q. cbv in H. meq; ra. rewrite <- !associative. ra.
Qed.

qrotv q v = (q2m q) * v

Lemma q2m_spec : forall (q : quat) (v : vec 3),
qunit q -> qrotv q v = ((q2m q) *v v)%M.
Proof. intros. unfold qrotv,qrot. q2e q. v2e v. veq; ra. Qed.

Definition Rsign (r : R) : R := if r >=? 0 then 1 else (-1).

Convert a rotation matrix to unit quaternion

Definition m2q (M : smat 3) : quat :=
  (let sign0 : R := 1 in
   let sign1 : R := sign0 * (Rsign (M.32 - M.23)) in
   let sign2 : R := sign0 * (Rsign (M.13 - M.31)) in
   let sign3 : R := sign0 * (Rsign (M.21 - M.12)) in
   l2v [sign0 * (1/2) * sqrt (1 + M.11 + M.22 + M.33);
        sign1 * (1/2) * sqrt (1 + M.11 - M.22 - M.33);
        sign2 * (1/2) * sqrt (1 - M.11 + M.22 - M.33);
        sign3 * (1/2) * sqrt (1 - M.11 - M.22 + M.33)])%R.

morth M -> qunit (m2q M)

Lemma m2q_qunit : forall (M : smat 3), morth M -> qunit (m2q M).
Proof.
  intros.
  apply morth_iff_mrowsOrthonormal in H.
  hnf in H. destruct H as [H1 H2]. hnf in H1,H2.
  assert (@nat2finS 2 0 <> #1) as H01 by fin.
  assert (@nat2finS 2 0 <> #2) as H02 by fin.
  assert (@nat2finS 2 1 <> #2) as H12 by fin.
  pose proof (H1 #0 #1 H01). pose proof (H1 #0 #2 H02). pose proof (H1 #1 #2 H12).
  pose proof (H2 #0). pose proof (H2 #1). pose proof (H2 #2).
  clear H1 H2 H01 H02 H12.
  v2eALL M.
  rename a0 into a11. rename a4 into a12. rename a5 into a13.
  rename a1 into a21. rename a6 into a22. rename a7 into a23.
  rename a2 into a31. rename a8 into a32. rename a9 into a33.
  cbv in *.
  apply sqrt_eq1_if_eq1. field_simplify. autorewrite with R in *.
  rewrite !Rsqr_sqrt. autoRbool; ra.
Admitted.

Lemma q2m_m2q_id : forall (M : smat 3), morth M -> q2m (m2q M) = M.
Proof.
  intros.
  v2eALL M.
  rename a0 into a11. rename a4 into a12. rename a5 into a13.
  rename a1 into a21. rename a6 into a22. rename a7 into a23.
  rename a2 into a31. rename a8 into a32. rename a9 into a33.
  meq.
  all: ring_simplify; try rewrite !pow2_sqrt.
  all: autoRbool; try lra.
  5:{
    ra. cbv. field_simplify_eq.
    replace (R1 + a11 + - a22 + - a33)%R with ((1 - a33) + (a11 - a22))%R by ring.
    replace (R1 + - a11 + a22 + - a33)%R with ((1 - a33) - (a11 - a22))%R by ring.
    replace (R1 + a11 + a22 + a33)%R with ((1 + a33) + (a11 + a22))%R by ring.
    replace (R1 + - a11 + -a22 + a33)%R with ((1 + a33) - (a11 + a22))%R by ring.
    rewrite !Rmult_assoc. rewrite <- !sqrt_mult_alt.
Admitted.

Lemma m2q_spec : forall (M : smat 3) (v : vec 3),
    morth M -> (M *v v)%M = qrotv (m2q M) v.
Proof.
  intros. rewrite q2m_spec. f_equiv.
  - rewrite q2m_m2q_id; easy.
  - apply m2q_qunit; auto.
Qed.

Section euler2quat.
  Variable euler : vec 3.
  Notation ϕ := (euler.1). Notation θ := (euler.2). Notation ψ := (euler.3).
  Notation ϕ2 := (ϕ /2). Notation θ2 := (θ /2). Notation ψ2 := (ψ /2).
  Notation cϕ2 := (cos ϕ2). Notation sϕ2 := (sin ϕ2).
  Notation cθ2 := (cos θ2). Notation sθ2 := (sin θ2).
  Notation c ψ2 := (cos ψ2). Notation s ψ2 := (sin ψ2).

  Definition euler2quat : quat :=
    l2v [cϕ2 * cθ2 * c ψ2 + sϕ2 * sθ2 * s ψ2 ;
         sϕ2 * cθ2 * c ψ2 - cϕ2 * sθ2 * s ψ2 ;
         cϕ2 * sθ2 * c ψ2 + sϕ2 * cθ2 * s ψ2 ;
         cϕ2 * cθ2 * s ψ2 - sϕ2 * sθ2 * c ψ2 ]%R.

  Let qx := aa2quat (mkAA v3i ϕ).
  Let qy := aa2quat (mkAA v3j θ).
  Let qz := aa2quat (mkAA v3k ψ).

  Lemma euler2quat_eq : euler2quat = qz * qy * qx.
  Proof. intros. unfold qx,qy,qz, euler2quat. v2e euler. veq; ra. Qed.

  Lemma euler2quat_spec : forall v, qrot euler2quat v = qrot qz (qrot qy (qrot qx v)).
  Proof. intros. rewrite !qrot_twice. rewrite euler2quat_eq. auto. Qed.
End euler2quat.

Section quat2euler.
  Variable q : quat.
  Notation q0 := (q.W). Notation q1 := (q.X).
  Notation q2 := (q.Y). Notation q3 := (q.Z).

  Definition quat2euler : vec 3 :=
    let phi := atan ((2 * (q0 * q1 + q2 * q3 )) / (1 - 2 * (q1 ² + q2 ² ))) in
    let theta := asin (2 * (q0 * q2 - q1 * q3 )) in
    let psi := atan ((2 * (q0 * q3 + q1 * q2 )) / (1 - 2 * (q2 ² + q3 ² ))) in
    l2v [phi ; theta ; psi ].

End quat2euler.