diff --git a/src/FSharp/FSharp.fsproj b/src/FSharp/FSharp.fsproj index 19964195..c6ffa0b9 100644 --- a/src/FSharp/FSharp.fsproj +++ b/src/FSharp/FSharp.fsproj @@ -74,9 +74,9 @@ + - diff --git a/src/FSharp/Quaternion.fs b/src/FSharp/Quaternion.fs index 2974a021..6aaa25fb 100644 --- a/src/FSharp/Quaternion.fs +++ b/src/FSharp/Quaternion.fs @@ -1,28 +1,56 @@ -module MathNet.Numerics.Quaternion +// +// Math.NET Numerics, part of the Math.NET Project +// http://numerics.mathdotnet.com +// http://github.com/mathnet/mathnet-numerics +// http://mathnetnumerics.codeplex.com +// +// Copyright (c) 2009-2016 Math.NET +// +// Permission is hereby granted, free of charge, to any person +// obtaining a copy of this software and associated documentation +// files (the "Software"), to deal in the Software without +// restriction, including without limitation the rights to use, +// copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the +// Software is furnished to do so, subject to the following +// conditions: +// +// The above copyright notice and this permission notice shall be +// included in all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES +// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND +// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT +// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +// OTHER DEALINGS IN THE SOFTWARE. +// -//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 +namespace MathNet.Numerics -type Quaternion = +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) @@ -31,60 +59,71 @@ type Quaternion = 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 + +[] +module 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 + + 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'. diff --git a/src/FSharpUnitTests/FSharpUnitTests.fsproj b/src/FSharpUnitTests/FSharpUnitTests.fsproj index 01ef3988..3a93e721 100644 --- a/src/FSharpUnitTests/FSharpUnitTests.fsproj +++ b/src/FSharpUnitTests/FSharpUnitTests.fsproj @@ -85,9 +85,9 @@ + - $(MSBuildExtensionsPath32)\Microsoft\VisualStudio\v$(VisualStudioVersion)\FSharp\Microsoft.FSharp.Targets diff --git a/src/FSharpUnitTests/QuaternionTests.fs b/src/FSharpUnitTests/QuaternionTests.fs index b37fd099..33ae1e5f 100644 --- a/src/FSharpUnitTests/QuaternionTests.fs +++ b/src/FSharpUnitTests/QuaternionTests.fs @@ -3,7 +3,7 @@ open System open NUnit.Framework open FsUnit -open MathNet.Numerics.Quaternion +open MathNet.Numerics module QuaternionTests = @@ -17,15 +17,15 @@ module QuaternionTests = [] let ``Quaternion.create`` () = let fourtyFiveDegreesInRadians = 45.0 * Math.PI / 180.0 - create fourtyFiveDegreesInRadians 1.0 0.0 0.0 |> + Quaternion.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 + Quaternion.create fourtyFiveDegreesInRadians 100.0 0.0 0.0 |> + should equal <| + Quaternion.create fourtyFiveDegreesInRadians 1.0 0.0 0.0 [] let ``Quaternion.+`` () = @@ -40,30 +40,30 @@ module QuaternionTests = r * q |> should equal {w = -8.0; x = -16.0; y = -24.0; z = -2.0;} [] - let ``Quaternion.* q*r`` () = + 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 + Quaternion.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} + Quaternion.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} + Quaternion.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} + Quaternion.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 + Quaternion.rotate n 1.0 1.0 1.0 |> should equal {w= 0.0;x= -1.0;y= 1.0;z= 1.0}