diff --git a/src/FSharp/FSharp-Net35.fsproj b/src/FSharp/FSharp-Net35.fsproj index f02ff997..ab274e03 100644 --- a/src/FSharp/FSharp-Net35.fsproj +++ b/src/FSharp/FSharp-Net35.fsproj @@ -75,6 +75,7 @@ + diff --git a/src/FSharp/FSharp.fsproj b/src/FSharp/FSharp.fsproj index cec10f18..19964195 100644 --- a/src/FSharp/FSharp.fsproj +++ b/src/FSharp/FSharp.fsproj @@ -76,6 +76,7 @@ + diff --git a/src/FSharp/Quaternion.fs b/src/FSharp/Quaternion.fs new file mode 100644 index 00000000..2974a021 --- /dev/null +++ b/src/FSharp/Quaternion.fs @@ -0,0 +1,90 @@ +module MathNet.Numerics.Quaternion + +//Reference: +//http://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf +//http://www.mathworks.com/help/aeroblks/quaternionmultiplication.html +//http://www.mathworks.com/help/aeroblks/quaterniondivision.html +//https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#The_conjugation_operation + +type Quaternion = + { + w:float + x:float + y:float + z:float + } with + 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} + 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} + 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} + 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} + static member (/) (q:Quaternion, a) = + {w=q.w/a; x=q.x/a;y=q.y/a;z=q.z/a} + +let norm q = + q.w**2.0 + q.x**2.0 + q.y**2.0 + q.z**2.0 + +let normalize q = + let invNorm = 1.0 / (norm q |> sqrt) + {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} + +let inverse q = + conjugate q / norm q + +//create a new quaternion +//angle in radians +//http://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf +//6.12 Unit Quaternion ⇐ Axis-Angle +// _ _ +// qa (α, n) := | cos α/2 | +// | n sin α/2 | +// - - +let create (angle:float) (x:float) (y:float) (z:float) = + //axis needs to be unit vector + let vNorm = x**2.0+y**2.0+z**2.0 + let invNorm = 1.0 / (vNorm |> sqrt) + let x' = x*invNorm + let y' = y*invNorm + let z' = z*invNorm + + let halfAngle = angle * 0.5 + let s = halfAngle |> sin + let c = halfAngle |> cos + {w=c; x=x'*s; y=y'*s; z=z'*s} + +//dot product +let dot q1 q2 = + q1.w*q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + +//rotate a vector(x,y,z) by a quaternion +// p is a pure quaternion(i.e w=0.0) +// p' = qpq**-1.0 +// https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#The_conjugation_operation +let rotate q1 x y z = + let q = normalize q1 //ensure unit quaternion + let p = {w=0.0; x=x; y=y; z=z} + q * p * inverse q + +/// +/// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation. +/// +/// The first Quaternion rotation in the series. +/// The second Quaternion rotation in the series. +/// A new Quaternion representing the concatenation of the value1 rotation followed by the value2 rotation. +let concat (q:Quaternion) (q':Quaternion) = q' * q //concat rotation is actually q' * q instead of q * q'. \ No newline at end of file diff --git a/src/FSharpUnitTests/FSharpUnitTests-Net35.fsproj b/src/FSharpUnitTests/FSharpUnitTests-Net35.fsproj index 1f5b4f30..a306b49b 100644 --- a/src/FSharpUnitTests/FSharpUnitTests-Net35.fsproj +++ b/src/FSharpUnitTests/FSharpUnitTests-Net35.fsproj @@ -70,6 +70,7 @@ + diff --git a/src/FSharpUnitTests/FSharpUnitTests.fsproj b/src/FSharpUnitTests/FSharpUnitTests.fsproj index 776c552a..01ef3988 100644 --- a/src/FSharpUnitTests/FSharpUnitTests.fsproj +++ b/src/FSharpUnitTests/FSharpUnitTests.fsproj @@ -87,6 +87,7 @@ + $(MSBuildExtensionsPath32)\Microsoft\VisualStudio\v$(VisualStudioVersion)\FSharp\Microsoft.FSharp.Targets diff --git a/src/FSharpUnitTests/QuaternionTests.fs b/src/FSharpUnitTests/QuaternionTests.fs new file mode 100644 index 00000000..b37fd099 --- /dev/null +++ b/src/FSharpUnitTests/QuaternionTests.fs @@ -0,0 +1,69 @@ +namespace MathNet.Numerics.Tests + +open System +open NUnit.Framework +open FsUnit +open MathNet.Numerics.Quaternion + +module QuaternionTests = + + //simple quaternions + let n = {w=1.0;x=0.0;y=1.0;z=0.0} + let n' = {w= 0.5;x= -0.5;y= 0.5;z= 0.0} + //less simple quaternions + let r = {w=1.0;x=2.0;y=3.0;z=4.0} + let q = {w= -4.0;x= 3.0; y= -2.0; z= 1.0} + + [] + let ``Quaternion.create`` () = + let fourtyFiveDegreesInRadians = 45.0 * Math.PI / 180.0 + create fourtyFiveDegreesInRadians 1.0 0.0 0.0 |> + should equal {w = 0.92387953251128674; x = 0.38268343236508978; y = 0.0; z = 0.0;} + + [] + let ``Quaternion.create normalizes input vector`` () = + let fourtyFiveDegreesInRadians = 45.0 * Math.PI / 180.0 + create fourtyFiveDegreesInRadians 100.0 0.0 0.0 |> + should equal <| + create fourtyFiveDegreesInRadians 1.0 0.0 0.0 + + [] + let ``Quaternion.+`` () = + n + n' |> should equal {w= 1.5;x= -0.5;y= 1.5;z= 0.0} + + [] + let ``Quaternion.-`` () = + n - n' |> should equal {w= 0.5;x= 0.5;y= 0.5;z= 0.0} + + [] + let ``Quaternion.* r*q'`` () = + r * q |> should equal {w = -8.0; x = -16.0; y = -24.0; z = -2.0;} + + [] + let ``Quaternion.* q*r`` () = + q * r |> should equal {w = -8.0; x = 6.0; y = -4.0; z = -28.0;} + + //http://www.mathworks.com/help/aerotbx/ug/quatnorm.html + [] + let ``Quaternion.Norm`` () = + norm n' |> should equal 0.75 + + //http://www.mathworks.com/help/aerotbx/ug/quatnormalize.html + [] + let ``Quaternion.Normalize`` () = + normalize n |> should equal {w= 0.70710678118654746; x= 0.0; y= 0.70710678118654746; z= 0.0} + + //http://www.mathworks.com/help/aerotbx/ug/quatinv.html + [] + let ``Quaternion.Inverse`` () = + inverse n |> should equal {w= 0.5;x= 0.0;y= -0.5;z= 0.0} + + //http://www.mathworks.com/help/aerotbx/ug/quatconj.html + [] + let ``Quaternion.Conjugate`` () = + conjugate n |> should equal {w= 1.0;x= 0.0;y= -1.0;z= 0.0} + + //http://www.mathworks.com/help/aerotbx/ug/quatrotate.html + [] + let ``Quaternion.Rotate`` () = + rotate n 1.0 1.0 1.0 |> should equal {w= 0.0;x= -1.0;y= 1.0;z= 1.0} \ No newline at end of file diff --git a/src/UnitTests/UnitTests-Net35.csproj b/src/UnitTests/UnitTests-Net35.csproj index 7e2b046b..9cd10e22 100644 --- a/src/UnitTests/UnitTests-Net35.csproj +++ b/src/UnitTests/UnitTests-Net35.csproj @@ -98,4 +98,7 @@ True + + + \ No newline at end of file