@ -39,32 +39,31 @@ type Quaternion =
} w i t h
static member (+) (r: Quaternion, q: Quaternion) =
{ w = r . w + q . w ; x = r . x + q . x ; y = r . y + q . y ; z = r . z + q . z }
{ w=r.w+q.w; x=r.x+q.x; y=r.y+q.y; z=r.z+q.z }
static member (-) (r: Quaternion, q: Quaternion) =
{ w = r . w - q . w ; x = r . x - q . x ; y = r . y - q . y ; z = r . z - q . z }
{ w=r.w-q.w; x=r.x-q.x; y=r.y-q.y; z=r.z-q.z }
static member (*) (r: Quaternion, q: Quaternion) =
let w = r.w*q.w - r.x*q.x - r.y*q.y - r . z * q . z
let x = r.w*q.x + r.x*q.w - r.y*q.z + r . z * q . y
let y = r.w*q.y + r.x*q.z + r.y*q.w - r . z * q . x
let z = r.w*q.z - r.x*q.y + r.y*q.x + r . z * q . w
{ w = w ; x = x ; y = y ; z = z }
{ w=w; x=x; y=y; z=z }
static member (/) (r: Quaternion, q: Quaternion) =
let d = (r.w**2.0 + r.x**2.0 + r.y**2.0 + r . z * * 2 .0 )
let w = (r.w*q.w + r.x*q.x + r.y*q.y + r.z*q.z) / d
let x = (r.w*q.x - r.x*q.w - r.y*q.z + r.z*q.y) / d
let y = (r.w*q.y + r.x*q.z - r.y*q.w - r.z*q.x) / d
let z = (r.w*q.z - r.x*q.y + r.y*q.x - r.z*q.w) / d
{ w = w ; x = x ; y = y ; z = z }
{ w=w; x=x; y=y; z=z }
static member (/) (q:Quaternion, a) =
{w=q.w/a; x = q . x / a ; y = q . y / a ; z = q . z / a }
{ w=q.w/a; x=q.x/a; y=q.y/a; z=q.z/a }
[<RequireQualifiedAccess; C o m p i l a t i o n R e p r e s e n t a t i o n ( C o m p i l a t i o n R e p r e s e n t a t i o n F l a g s . M o d u l e S u f f i x ) > ]
[ < C o m p i l a t i o n R e p r e s e n t a t i o n ( C o m p i l a t i o n R e p r e s e n t a t i o n F l a g s . M o d u l e S u f f i x ) > ]
module Quaternion =
/ / R e f e r e n c e :
@ -74,21 +73,27 @@ module Quaternion =
/ / h t t p s : / / e n . w i k i p e d i a . o r g / w i k i / Q u a t e r n i o n s _ a n d _ s p a t i a l _ r o t a t i o n # T h e _ c o n j u g a t i o n _ o p e r a t i o n
let normSquared q =
q.w**2.0 + q.x**2.0 + q.y**2.0 + q . z * * 2 .0
q.w*q.w + q.x*q.x + q.y*q.y + q . z * q . z
/// Euclidean N o r m
let norm q =
normSquared q |> s q r t
/// Normalize a quaternion to unit quaternion with norm 1 .
let normalize q =
let invNorm = 1 .0 / (norm q )
{ w = q . w * i n v N o r m ; x = q . x * i n v N o r m ; y = q . y * i n v N o r m ; z = q . z * i n v N o r m }
{ w=q.w*invNorm; x=q.x*invNorm; y=q.y*invNorm; z=q.z*invNorm }
let conjugate q =
{w=q.w; x= -q.x; y= -q.y; z= - q . z }
{ w=q.w; x= -q.x; y= -q.y; z= -q.z }
let inverse q =
conjugate q / normSquared q
/// Dot p r o d u c t
let dot q1 q2 =
q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q 1 . z * q 2 . z
//create a new q u a t e r n i o n
//angle in r a d i a n s
/ / h t t p : / / w w w . a s t r o . r u g . n l / s o f t w a r e / k a p t e y n / _ d o w n l o a d s / a t t i t u d e . p d f
@ -96,28 +101,26 @@ module Quaternion =
// _ _
// qa (α, n) := | cos α/2 |
// | n sin α/2 |
// - -
/ /
[<System.Obsolete("Semantic Version Opt-Out: this routine has not been finalized yet and may change in breaking ways within minor v e r s i o n s . " ) > ]
let create (angle:float) (x:float) (y:float) (z:float) =
//axis needs to be unit v e c t o r
let vNorm = x * * 2 .0 + y * * 2 .0 + z * * 2 .0
let invNorm = 1 .0 / (vNorm |> s q r t )
let vNorm = x*x + y*y + z * z
let invNorm = 1 .0 / (sqrt v N o r m )
let x' = x * i n v N o r m
let y' = y * i n v N o r m
let z' = z * i n v N o r m
let halfAngle = angle * 0 .5
let s = halfAngle |> s i n
let c = halfAngle |> c o s
{w=c; x=x'*s; y=y'*s; z = z ' * s }
//dot p r o d u c t
let dot q1 q2 =
q1.w*q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q 2 . z
//rotate a vector(x,y,z) by a q u a t e r n i o n
// p is a pure quaternion(i.e w = 0 .0 )
// p' = q p q * * -1 .0
// h t t p s : / / e n . w i k i p e d i a . o r g / w i k i / Q u a t e r n i o n s _ a n d _ s p a t i a l _ r o t a t i o n # T h e _ c o n j u g a t i o n _ o p e r a t i o n
let s = sin h a l f A n g l e
let c = cos h a l f A n g l e
{ w=c; x=x'*s; y=y'*s; z=z'*s }
/// rotate a vector(x,y,z) by a q u a t e r n i o n
/// p is a pure quaternion(i.e w = 0 .0 )
/// p' = q p q * * -1 .0
/// h t t p s : / / e n . w i k i p e d i a . o r g / w i k i / Q u a t e r n i o n s _ a n d _ s p a t i a l _ r o t a t i o n # T h e _ c o n j u g a t i o n _ o p e r a t i o n
[<System.Obsolete("Semantic Version Opt-Out: this routine has not been finalized yet and may change in breaking ways within minor v e r s i o n s . " ) > ]
let rotate q1 x y z =
let q = normalize q1 //ensure unit q u a t e r n i o n
let p = {w=0.0; x=x; y=y; z = z }
@ -129,4 +132,7 @@ module Quaternion =
/// <param name="value1">The first Quaternion rotation in the s e r i e s . < / p a r a m >
/// <param name="value2">The second Quaternion rotation in the s e r i e s . < / p a r a m >
/// <returns>A new Quaternion representing the concatenation of the value1 rotation followed by the value2 r o t a t i o n . < / r e t u r n s >
let concat (q:Quaternion) (q':Quaternion) = q' * q //concat rotation is actually q' * q instead of q * q ' .
[<System.Obsolete("Semantic Version Opt-Out: this routine has not been finalized yet and may change in breaking ways within minor v e r s i o n s . " ) > ]
let concat (q:Quaternion) (q':Quaternion) =
//concat rotation is q' * q instead of q * q ' .
q' * q