OrienRepr.OrienRepr

Require Export RotationMatrix3D AxisAngle EulerAngle Quaternion.

Open Scope R.
Open Scope quat_scope.
Open Scope mat_scope.
Open Scope vec_scope.

Comparison of Extrinsic and Intrinsic Rotation

Module xyzRPY_eq_zyxEULER.

  Definition xyzEXTR (angx angy angz : R) : smat 3 := Rz angz * Ry angy * Rx angx.

  Section xyzEXTR_spec.
    Variable p : vec 3.

    Variable angx angy angz : R.
    Let p1 := Rx angx *v p.
    Let p2 := Ry angy *v p1.
    Let p' := Rz angz *v p2.

    Lemma xyzEXTR_spec : p' = (xyzEXTR angx angy angz) *v p.
    Proof. unfold xyzEXTR, p', p2, p1. rewrite !mmulv_assoc. auto. Qed.
  End xyzEXTR_spec.

  Definition zyxINTR (angx angy angz : R) : smat 3 := Rz angz * Ry angy * Rx angx.

  Section zyxINTR_spec.
    Variable p : vec 3.

    Variable angx angy angz : R.
    Let q1 := Rz angz *v p.
    Let q2 := Rz angz *v Ry angy *v ((Rz angz)\-1) *v q1.
    Let p'' := (Rz angz * Ry angy)
               *v Rx angx
               *v ((Rz angz * Ry angy)\-1)
               *v q2.

    Fact q2_eq : q2 = Rz angz *v Ry angy *v p.
    Proof.
      unfold q2, q1. rewrite <- !mmulv_assoc. rewrite mmul_assoc.
      rewrite mmul_minvAM_l. rewrite mmul_1_r. auto. apply Rz_minvtble.
    Qed.

    Lemma zyxINTR_spec : p'' = (zyxINTR angx angy angz) *v p.
    Proof.
      unfold zyxINTR, p''. rewrite q2_eq.
      rewrite <- !mmulv_assoc. rewrite !mmul_assoc.
      rewrite mmul_minvAM_l. rewrite mmul_1_r. auto.
      apply mmul_minvtble. apply Rz_minvtble. apply Ry_minvtble.
    Qed.
  End zyxINTR_spec.

  Lemma xyzEXTR_eq_zyxINTR : forall angx angy angz,
      xyzEXTR angx angy angz = zyxINTR angx angy angz.
  Proof. auto. Qed.

End xyzRPY_eq_zyxEULER.

Rotate a vector in a given frame to obtain new coordinates

Rotates vector `v` along the x-axis by `ang` (radians)

Definition rotx (ang : R) : vec 3 -> vec 3 := fun v => (Rx ang) *v v.

Rotates vector `v` along the y-axis by `ang` (radians)

Definition roty (ang : R) : vec 3 -> vec 3 := fun v => (Ry ang) *v v.

Rotates vector `v` along the z-axis by `ang` (radians)

Definition rotz (ang : R) : vec 3 -> vec 3 := fun v => (Rz ang) *v v.

Rotates vector `v` along the `ax`-axis by `ang` (radians)

Definition rotaa (aa : vec 4) : vec 3 -> vec 3 := fun v => rotaa (v2aa aa) v.

Rotates vector `v` by rotation matrix `M`

Definition rotByM (M : smat 3) (v : vec 3) : vec 3 := M *v v.

Rotates vector `v` by unit quaternion `q`

Definition rotByQ (q : vec 4) (v : vec 3) : vec 3 := qrotv q v.

Rotates vector `v` by unit quaternion `q1` and followed by q2

Definition rot2ByQ (q1 q2 : vec 4) (v : vec 3) : vec 3 := qrotv (q2 * q1)%quat v.

Conversion between ORs

Euler angles <-> Rotation matrix

Euler angles (roll,pitch,yaw) to rotation matrix

Definition e2m (euler : vec 3) : smat 3 := S123 (euler.1) (euler.2) (euler.3).

Rotation matrix to euler angles. Note that M must belogn to SO(3)

Definition m2e (M : smat 3) : vec 3 :=
l2v [R2Euler_S123.alg2.ϕ' M ; R2Euler_S123.alg2.θ' M ; R2Euler_S123.alg2 .ψ' M ].

Axis-angle <-> Rotation matrix

Axis-angle to rotation matrix by rodrigues formula. Note that axis must be unit

Definition a2m (aa : vec 4) : smat 3 := aa2mat (v2aa aa).

Axis-angle to rotation matrix by direct formula. Note that axis must be unit

Definition a2m' (aa : vec 4) : smat 3 := aa2matM (v2aa aa).

Rotation matrix to axis-angle (NOT SUPPORTED YET)

Axis-angle to Euler angles. Note that axis must be unit

Definition a2e (aa : vec 4) : vec 3 := m2e (a2m aa).

Unit quaternion <-> Rotation matrix

Unit quaternion to rotation matrix. Note that q must be unit

Rotation matrix to unit quaternion. Note that M must belong to SO(3)

Unit quaternion <-> Euler angles

Unit quaternion to euler angles. Note that q must be unit

Definition q2e (q : vec 4) : vec 3 := m2e (q2m q).

Euler angles to unit quaternion.

Definition e2q (e : vec 3) : vec 4 := m2q (e2m e).

Unit quaternion <-> Axis-angle

Unit quaternion to Axis-angle. Note that q must be unit.

Definition q2a (q : vec 4) : vec 4 := aa2v (quat2aa q).

Axis-angle to quaternion. Note that axis must be unit

Definition a2q (aa : vec 4) : vec 4 := aa2quat (v2aa aa).

Examples

我们的库是简单的而又有统一类型的： 1. 简单性：与数学惯例保持一致的记号，如 v.1, v.2, m.11, m.12 等 2. 类型统一性，以及数学定义的直观性（相比于MathComp的做法）：

向量就是向量，而不是列矩阵或行矩阵，我们提供 mmulv 和 mvmul 运算，

同时也提供 cvec, rvec 等类型。

点积、叉积等运算不需要借助矩阵才能实现，而是直接定义

Print v3cross.

四元数是vec 4，做了一些改进。

在Coq中可计算带来了符号推导的好处

Section executability_for_symbol_derivation.
  Open Scope R_scope.
  Variable ϕ θ ψ : R.

  Variable a11 a12 a13 a21 a22 a23 a31 a32 a33 : R.
  Let A : smat 3 := l2m [[a11 ;a12 ;a13 ];[a21 ;a22 ;a23 ];[a31 ;a32 ;a33 ]].

  Goal A = (Rz ψ * Ry θ * Rx ϕ )%M.
  Proof.
    meq; autorewrite with R.
  Abort.
End executability_for_symbol_derivation.

还可以抽取出 OCaml 程序，见下一节

Extraction to OCaml code

Extraction "ocaml_orienRepr.ml"
  v2l l2v m2l l2m
  rotx roty rotz rotaa rotByM rotByQ rot2ByQ
  e2m e2q
  a2m a2e a2q
  q2e q2m q2a
  m2e m2q.