Merge pull request #367 from pdoh00/Quaternion

Added Quaternion and QuaternionTests to the FSharp project

src/FSharp/FSharp-Net35.fsproj

@@ -75,6 +75,7 @@
     <Compile Include="FindRoots.fs" />
     <Compile Include="RandomVariable.fs" />
     <None Include="MathNet.Numerics.fsx" />
+    <Compile Include="Quaternion.fs" />
   </ItemGroup>
   <ItemGroup>
     <Reference Include="FSharp.Core, Version=2.3.0.0, Culture=neutral, PublicKeyToken=b03f5f7f11d50a3a">

src/FSharp/FSharp.fsproj

@@ -76,6 +76,7 @@
     <Compile Include="RandomVariable.fs" />
     <None Include="MathNet.Numerics.fsx" />
     <None Include="MathNet.Numerics.IfSharp.fsx" />
+    <Compile Include="Quaternion.fs" />
   </ItemGroup>
   <ItemGroup>
     <Reference Include="mscorlib" />

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
+
+/// <summary>
+/// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation.
+/// </summary>
+/// <param name="value1">The first Quaternion rotation in the series.</param>
+/// <param name="value2">The second Quaternion rotation in the series.</param>
+/// <returns>A new Quaternion representing the concatenation of the value1 rotation followed by the value2 rotation.</returns>
+let concat (q:Quaternion) (q':Quaternion) = q' * q //concat rotation is actually q' * q instead of q * q'.

src/FSharpUnitTests/FSharpUnitTests-Net35.fsproj

@@ -70,6 +70,7 @@
     <Compile Include="FitTests.fs" />
     <Compile Include="FindRootsTests.fs" />
     <None Include="paket.references" />
+    <Compile Include="QuaternionTests.fs" />
   </ItemGroup>
   <ItemGroup>
     <Reference Include="mscorlib" />

src/FSharpUnitTests/FSharpUnitTests.fsproj

@@ -87,6 +87,7 @@
     <Compile Include="FindRootsTests.fs" />
     <None Include="paket.references" />
     <None Include="App.config" />
+    <Compile Include="QuaternionTests.fs" />
   </ItemGroup>
   <PropertyGroup>
     <FSharpTargetsPath>$(MSBuildExtensionsPath32)\Microsoft\VisualStudio\v$(VisualStudioVersion)\FSharp\Microsoft.FSharp.Targets</FSharpTargetsPath>

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}
+
+    [<Test>]
+    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;}
+
+    [<Test>]
+    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 
+
+    [<Test>]
+    let ``Quaternion.+`` () =
+        n + n' |> should equal {w= 1.5;x= -0.5;y= 1.5;z= 0.0}
+
+    [<Test>]
+    let ``Quaternion.-`` () =
+        n - n' |> should equal {w= 0.5;x= 0.5;y= 0.5;z= 0.0}
+
+    [<Test>]
+    let ``Quaternion.* r*q'`` () =
+        r * q |> should equal  {w = -8.0; x = -16.0; y = -24.0; z = -2.0;}
+
+    [<Test>]
+    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
+    [<Test>]
+    let ``Quaternion.Norm`` () =
+        norm n' |> should equal 0.75
+
+    //http://www.mathworks.com/help/aerotbx/ug/quatnormalize.html
+    [<Test>]
+    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
+    [<Test>]
+    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
+    [<Test>]
+    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
+    [<Test>]
+    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}

src/UnitTests/UnitTests-Net35.csproj

@@ -98,4 +98,7 @@
       <Paket>True</Paket>
     </Reference>
   </ItemGroup>
+  <ItemGroup>
+    <Service Include="{82A7F48D-3B50-4B1E-B82E-3ADA8210C358}" />
+  </ItemGroup>
 </Project>