OrienRepr.QuaternionAdvanced

Require Export Quaternion.

Section qdot.

  Definition qdot (q1 q2 : quat) : quat :=
    l2v [q1.W * q1.W; q1.X * q2.X; q1.Y * q2.Y; q1.Z * q2.Z]%R.

End qdot.

Section qlog.

  Parameter qlog : quat -> quat.

End qlog.

Section qexp.

  Parameter qexp : quat -> quat.

  Axiom qexp_qlog : forall q : quat, qexp (qlog q) = q.

End qexp.

Section qpower.



  Definition qpower' (q : quat) (t : R) : quat := qexp (t s* qlog q).



  Definition qpower (q : quat) (exponent : R) : quat :=
    if (Rabs (q.X) <? 0.9999)
    then
      (let alpha : R := acos (q.W) in
       let newAlpha : R := (alpha * exponent)%R in
       let mult : R := (sin newAlpha) / (sin alpha) in
       l2v [cos newAlpha; q.X * mult; q.Y * mult; q.Z * mult]%R)
    else q.

End qpower.

Section qslerp.

  Definition qslerp_cosOmega (q0 q1 : quat) : quat * quat * R :=
    
    let cosOmega : R := (q0.W * q1.W + q0.X * q1.X + q0.Y * q1.Y + q0.Z * q1.Z)%R in
    
    if (cosOmega <? 0)
    then (q0, -q1, (-cosOmega)%R)
    else (q0, q1, cosOmega).

  Definition qslerp_parameter (q0 q1 : quat) (cosOmega : R) (t : R) : R * R :=
    
    if (cosOmega >? 0.9999)
    then (
        let k0 : R := (1 - t)%R in
        let k1 : R := t in
        (k0, k1))
    else (
        
        let sinOmega : R := sqrt (1 - cosOmega * cosOmega) in
        
        let omega : R := atan2 sinOmega cosOmega in
        
        let oneOverSinOmega : R := 1 / sinOmega in
        let k0 : R := (sin ((1 - t) * omega) * oneOverSinOmega)%R in
        let k1 : R := (sin (t * omega) * oneOverSinOmega)%R in
        (k0, k1)).

  Definition qslerp (q0 q1 : quat) (t : R) : quat :=
    
    let '(q0,q1,cosOmega) := qslerp_cosOmega q0 q1 in
    
    let '(k0, k1) := qslerp_parameter q0 q1 cosOmega t in
    
    l2v [q0.W * k0 + q1.W * k1;
         q0.X * k0 + q1.X * k1;
         q0.Y * k0 + q1.Y * k1;
         q0.Z * k0 + q1.Z * k1]%R.

End qslerp.