Update to Elixir 1.6
* Run code formatter. * Credo doesn't like `mix format`'s line length. * Fix failing tests re `Angle`.
This commit is contained in:
parent
1259f263ed
commit
f74836dfd3
38 changed files with 537 additions and 332 deletions
146
.credo.exs
Normal file
146
.credo.exs
Normal file
|
@ -0,0 +1,146 @@
|
|||
# This file contains the configuration for Credo and you are probably reading
|
||||
# this after creating it with `mix credo.gen.config`.
|
||||
#
|
||||
# If you find anything wrong or unclear in this file, please report an
|
||||
# issue on GitHub: https://github.com/rrrene/credo/issues
|
||||
#
|
||||
%{
|
||||
#
|
||||
# You can have as many configs as you like in the `configs:` field.
|
||||
configs: [
|
||||
%{
|
||||
#
|
||||
# Run any exec using `mix credo -C <name>`. If no exec name is given
|
||||
# "default" is used.
|
||||
#
|
||||
name: "default",
|
||||
#
|
||||
# These are the files included in the analysis:
|
||||
files: %{
|
||||
#
|
||||
# You can give explicit globs or simply directories.
|
||||
# In the latter case `**/*.{ex,exs}` will be used.
|
||||
#
|
||||
included: ["lib/", "src/", "web/", "apps/"],
|
||||
excluded: [~r"/_build/", ~r"/deps/"]
|
||||
},
|
||||
#
|
||||
# If you create your own checks, you must specify the source files for
|
||||
# them here, so they can be loaded by Credo before running the analysis.
|
||||
#
|
||||
requires: [],
|
||||
#
|
||||
# If you want to enforce a style guide and need a more traditional linting
|
||||
# experience, you can change `strict` to `true` below:
|
||||
#
|
||||
strict: false,
|
||||
#
|
||||
# If you want to use uncolored output by default, you can change `color`
|
||||
# to `false` below:
|
||||
#
|
||||
color: true,
|
||||
#
|
||||
# You can customize the parameters of any check by adding a second element
|
||||
# to the tuple.
|
||||
#
|
||||
# To disable a check put `false` as second element:
|
||||
#
|
||||
# {Credo.Check.Design.DuplicatedCode, false}
|
||||
#
|
||||
checks: [
|
||||
{Credo.Check.Consistency.ExceptionNames},
|
||||
{Credo.Check.Consistency.LineEndings},
|
||||
{Credo.Check.Consistency.ParameterPatternMatching},
|
||||
{Credo.Check.Consistency.SpaceAroundOperators},
|
||||
{Credo.Check.Consistency.SpaceInParentheses},
|
||||
{Credo.Check.Consistency.TabsOrSpaces},
|
||||
|
||||
# You can customize the priority of any check
|
||||
# Priority values are: `low, normal, high, higher`
|
||||
#
|
||||
{Credo.Check.Design.AliasUsage, priority: :low},
|
||||
|
||||
# For some checks, you can also set other parameters
|
||||
#
|
||||
# If you don't want the `setup` and `test` macro calls in ExUnit tests
|
||||
# or the `schema` macro in Ecto schemas to trigger DuplicatedCode, just
|
||||
# set the `excluded_macros` parameter to `[:schema, :setup, :test]`.
|
||||
#
|
||||
{Credo.Check.Design.DuplicatedCode, excluded_macros: []},
|
||||
|
||||
# You can also customize the exit_status of each check.
|
||||
# If you don't want TODO comments to cause `mix credo` to fail, just
|
||||
# set this value to 0 (zero).
|
||||
#
|
||||
{Credo.Check.Design.TagTODO, exit_status: 2},
|
||||
{Credo.Check.Design.TagFIXME},
|
||||
|
||||
{Credo.Check.Readability.FunctionNames},
|
||||
{Credo.Check.Readability.LargeNumbers},
|
||||
{Credo.Check.Readability.MaxLineLength, priority: :low, max_length: 120},
|
||||
{Credo.Check.Readability.ModuleAttributeNames},
|
||||
{Credo.Check.Readability.ModuleDoc},
|
||||
{Credo.Check.Readability.ModuleNames},
|
||||
{Credo.Check.Readability.ParenthesesOnZeroArityDefs},
|
||||
{Credo.Check.Readability.ParenthesesInCondition},
|
||||
{Credo.Check.Readability.PredicateFunctionNames},
|
||||
{Credo.Check.Readability.PreferImplicitTry},
|
||||
{Credo.Check.Readability.RedundantBlankLines},
|
||||
{Credo.Check.Readability.StringSigils},
|
||||
{Credo.Check.Readability.TrailingBlankLine},
|
||||
{Credo.Check.Readability.TrailingWhiteSpace},
|
||||
{Credo.Check.Readability.VariableNames},
|
||||
{Credo.Check.Readability.Semicolons},
|
||||
{Credo.Check.Readability.SpaceAfterCommas},
|
||||
|
||||
{Credo.Check.Refactor.DoubleBooleanNegation},
|
||||
{Credo.Check.Refactor.CondStatements},
|
||||
{Credo.Check.Refactor.CyclomaticComplexity},
|
||||
{Credo.Check.Refactor.FunctionArity},
|
||||
{Credo.Check.Refactor.LongQuoteBlocks},
|
||||
{Credo.Check.Refactor.MatchInCondition},
|
||||
{Credo.Check.Refactor.NegatedConditionsInUnless},
|
||||
{Credo.Check.Refactor.NegatedConditionsWithElse},
|
||||
{Credo.Check.Refactor.Nesting},
|
||||
{Credo.Check.Refactor.PipeChainStart},
|
||||
{Credo.Check.Refactor.UnlessWithElse},
|
||||
|
||||
{Credo.Check.Warning.BoolOperationOnSameValues},
|
||||
{Credo.Check.Warning.ExpensiveEmptyEnumCheck},
|
||||
{Credo.Check.Warning.IExPry},
|
||||
{Credo.Check.Warning.IoInspect},
|
||||
{Credo.Check.Warning.LazyLogging},
|
||||
{Credo.Check.Warning.OperationOnSameValues},
|
||||
{Credo.Check.Warning.OperationWithConstantResult},
|
||||
{Credo.Check.Warning.UnusedEnumOperation},
|
||||
{Credo.Check.Warning.UnusedFileOperation},
|
||||
{Credo.Check.Warning.UnusedKeywordOperation},
|
||||
{Credo.Check.Warning.UnusedListOperation},
|
||||
{Credo.Check.Warning.UnusedPathOperation},
|
||||
{Credo.Check.Warning.UnusedRegexOperation},
|
||||
{Credo.Check.Warning.UnusedStringOperation},
|
||||
{Credo.Check.Warning.UnusedTupleOperation},
|
||||
{Credo.Check.Warning.RaiseInsideRescue},
|
||||
|
||||
# Controversial and experimental checks (opt-in, just remove `, false`)
|
||||
#
|
||||
{Credo.Check.Refactor.ABCSize, false},
|
||||
{Credo.Check.Refactor.AppendSingleItem, false},
|
||||
{Credo.Check.Refactor.VariableRebinding, false},
|
||||
{Credo.Check.Warning.MapGetUnsafePass, false},
|
||||
{Credo.Check.Consistency.MultiAliasImportRequireUse, false},
|
||||
|
||||
# Deprecated checks (these will be deleted after a grace period)
|
||||
#
|
||||
{Credo.Check.Readability.Specs, false},
|
||||
{Credo.Check.Warning.NameRedeclarationByAssignment, false},
|
||||
{Credo.Check.Warning.NameRedeclarationByCase, false},
|
||||
{Credo.Check.Warning.NameRedeclarationByDef, false},
|
||||
{Credo.Check.Warning.NameRedeclarationByFn, false},
|
||||
|
||||
# Custom checks can be created using `mix credo.gen.check`.
|
||||
#
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
|
@ -2,11 +2,13 @@ defimpl Inspect, for: Kinemat.Cartesian do
|
|||
alias Kinemat.Cartesian
|
||||
import Inspect.Algebra
|
||||
|
||||
@spec inspect(Cartesian.t, any) :: String.t
|
||||
@spec inspect(Cartesian.t(), any) :: String.t()
|
||||
def inspect(angle, opts) do
|
||||
angle = angle
|
||||
|> Map.from_struct
|
||||
angle =
|
||||
angle
|
||||
|> Map.from_struct()
|
||||
|> Enum.into([])
|
||||
concat ["#Kinemat.Point<", to_doc(angle, opts), ">"]
|
||||
|
||||
concat(["#Kinemat.Point<", to_doc(angle, opts), ">"])
|
||||
end
|
||||
end
|
||||
|
|
|
@ -2,11 +2,13 @@ defimpl Inspect, for: Kinemat.Cylindrical do
|
|||
alias Kinemat.Cylindrical
|
||||
import Inspect.Algebra
|
||||
|
||||
@spec inspect(Cylindrical.t, any) :: String.t
|
||||
@spec inspect(Cylindrical.t(), any) :: String.t()
|
||||
def inspect(angle, opts) do
|
||||
angle = angle
|
||||
|> Map.from_struct
|
||||
angle =
|
||||
angle
|
||||
|> Map.from_struct()
|
||||
|> Enum.into([])
|
||||
concat ["#Kinemat.Point<", to_doc(angle, opts), ">"]
|
||||
|
||||
concat(["#Kinemat.Point<", to_doc(angle, opts), ">"])
|
||||
end
|
||||
end
|
||||
|
|
|
@ -2,9 +2,9 @@ defimpl Inspect, for: Kinemat.Euler do
|
|||
alias Kinemat.{Euler, Orientation}
|
||||
import Inspect.Algebra
|
||||
|
||||
@spec inspect(Orientation.t, any) :: String.t
|
||||
@spec inspect(Orientation.t(), any) :: String.t()
|
||||
def inspect(%Euler{representation: r, x: x, y: y, z: z}, opts) do
|
||||
values = [euler: r, x: x, y: y, z: z]
|
||||
concat ["#Kinemat.Orientation<", to_doc(values, opts), ">"]
|
||||
concat(["#Kinemat.Orientation<", to_doc(values, opts), ">"])
|
||||
end
|
||||
end
|
||||
|
|
|
@ -2,11 +2,13 @@ defimpl Inspect, for: Kinemat.Frame do
|
|||
alias Kinemat.Frame
|
||||
import Inspect.Algebra
|
||||
|
||||
@spec inspect(Frame.t, any) :: String.t
|
||||
@spec inspect(Frame.t(), any) :: String.t()
|
||||
def inspect(plane, opts) do
|
||||
plane = plane
|
||||
|> Map.from_struct
|
||||
plane =
|
||||
plane
|
||||
|> Map.from_struct()
|
||||
|> Enum.into([])
|
||||
concat ["#Kinemat.Frame<", to_doc(plane, opts), ">"]
|
||||
|
||||
concat(["#Kinemat.Frame<", to_doc(plane, opts), ">"])
|
||||
end
|
||||
end
|
||||
|
|
|
@ -2,9 +2,9 @@ defimpl Inspect, for: Kinemat.Quaternion do
|
|||
alias Kinemat.{Quaternion, Orientation}
|
||||
import Inspect.Algebra
|
||||
|
||||
@spec inspect(Orientation.t, any) :: String.t
|
||||
@spec inspect(Orientation.t(), any) :: String.t()
|
||||
def inspect(%Quaternion{w: w, x: x, y: y, z: z}, opts) do
|
||||
values = [type: :quaternion, w: w, x: x, y: y, z: z]
|
||||
concat ["#Kinemat.Orientation<", to_doc(values, opts), ">"]
|
||||
concat(["#Kinemat.Orientation<", to_doc(values, opts), ">"])
|
||||
end
|
||||
end
|
||||
|
|
|
@ -2,13 +2,20 @@ defimpl Inspect, for: Kinemat.RotationMatrix do
|
|||
alias Kinemat.{RotationMatrix, Orientation}
|
||||
import Inspect.Algebra
|
||||
|
||||
@spec inspect(Orientation.t, any) :: String.t
|
||||
def inspect(%RotationMatrix{matrix: {m00, m01, m02, m10, m11, m12, m20, m21, m22}}, opts) do
|
||||
concat ["#Kinemat.Orientation<", to_doc([
|
||||
rotation_matrix: {
|
||||
{m00, m01, m02},
|
||||
{m10, m11, m12},
|
||||
{m20, m21, m22}}
|
||||
], opts), ">"]
|
||||
@spec inspect(Orientation.t(), any) :: String.t()
|
||||
def inspect(
|
||||
%RotationMatrix{matrix: {m00, m01, m02, m10, m11, m12, m20, m21, m22}},
|
||||
opts
|
||||
) do
|
||||
concat([
|
||||
"#Kinemat.Orientation<",
|
||||
to_doc(
|
||||
[
|
||||
rotation_matrix: {{m00, m01, m02}, {m10, m11, m12}, {m20, m21, m22}}
|
||||
],
|
||||
opts
|
||||
),
|
||||
">"
|
||||
])
|
||||
end
|
||||
end
|
||||
|
|
|
@ -1,11 +1,13 @@
|
|||
defimpl Inspect, for: Kinemat.Spherical do
|
||||
import Inspect.Algebra
|
||||
|
||||
@spec inspect(Angle.t, any) :: String.t
|
||||
@spec inspect(Angle.t(), any) :: String.t()
|
||||
def inspect(angle, opts) do
|
||||
angle = angle
|
||||
|> Map.from_struct
|
||||
angle =
|
||||
angle
|
||||
|> Map.from_struct()
|
||||
|> Enum.into([])
|
||||
concat ["#Kinemat.Point<", to_doc(angle, opts), ">"]
|
||||
|
||||
concat(["#Kinemat.Point<", to_doc(angle, opts), ">"])
|
||||
end
|
||||
end
|
||||
|
|
|
@ -1,10 +1,19 @@
|
|||
defmodule Kinemat do
|
||||
defmacro __using__(_opts) do
|
||||
quote do
|
||||
alias Kinemat.{Point, Cartesian, Cylindrical, Spherical,
|
||||
Orientation, Euler, RotationMatrix, Quaternion,
|
||||
# Joint, Point, Prismatic, Radians, Revolute, Spherical,
|
||||
Frame}
|
||||
alias Kinemat.{
|
||||
Point,
|
||||
Cartesian,
|
||||
Cylindrical,
|
||||
Spherical,
|
||||
Orientation,
|
||||
Euler,
|
||||
RotationMatrix,
|
||||
Quaternion,
|
||||
# Joint, Point, Prismatic, Radians, Revolute, Spherical,
|
||||
Frame
|
||||
}
|
||||
|
||||
import Angle.Sigil
|
||||
end
|
||||
end
|
||||
|
@ -21,5 +30,4 @@ defmodule Kinemat do
|
|||
...> Euler.init(:xyz, ~a(0), ~a(0), ~a(0))
|
||||
#Kinemat.Orientation<[euler: :xyz, x: #Angle<0>, y: #Angle<0>, z: #Angle<0>]>
|
||||
"""
|
||||
|
||||
end
|
||||
|
|
|
@ -6,9 +6,7 @@ defmodule Kinemat.Cartesian do
|
|||
Describes a point in three-dimensional space using cartesian coordinates (x, y, z).
|
||||
"""
|
||||
|
||||
@type t :: %Cartesian{x: number,
|
||||
y: number,
|
||||
z: number}
|
||||
@type t :: %Cartesian{x: number, y: number, z: number}
|
||||
|
||||
@doc """
|
||||
Initialise a cartesian point from `x`, `y` and `z`.
|
||||
|
|
|
@ -6,9 +6,7 @@ defmodule Kinemat.Cylindrical do
|
|||
Describes a point in cylindrical coordinates.
|
||||
"""
|
||||
|
||||
@type t :: %Cylindrical{radial: number,
|
||||
azimuth: Angle.t,
|
||||
vertical: number}
|
||||
@type t :: %Cylindrical{radial: number, azimuth: Angle.t(), vertical: number}
|
||||
|
||||
@doc """
|
||||
Initalise a cylindrical coordinate from `rho`, `theta`, `z` (ρ,θ,z).
|
||||
|
@ -25,7 +23,7 @@ defmodule Kinemat.Cylindrical do
|
|||
azimuth: ~a(0.5)r,
|
||||
vertical: 30}
|
||||
"""
|
||||
@spec init(number, Angle.t, number) :: t
|
||||
@spec init(number, Angle.t(), number) :: t
|
||||
def init(rho, %Angle{} = theta, z) do
|
||||
%Cylindrical{radial: rho, azimuth: theta, vertical: z}
|
||||
end
|
||||
|
@ -51,7 +49,7 @@ defmodule Kinemat.Cylindrical do
|
|||
@doc """
|
||||
Alias for `azimuth/1`.
|
||||
"""
|
||||
@spec theta(t) :: Angle.t
|
||||
@spec theta(t) :: Angle.t()
|
||||
def theta(point), do: point |> azimuth()
|
||||
|
||||
@doc """
|
||||
|
@ -63,7 +61,7 @@ defmodule Kinemat.Cylindrical do
|
|||
...> |> Cylindrical.azimuth()
|
||||
#Angle<0.5㎭>
|
||||
"""
|
||||
@spec azimuth(t) :: Angle.t
|
||||
@spec azimuth(t) :: Angle.t()
|
||||
def azimuth(%Cylindrical{azimuth: theta}), do: theta
|
||||
|
||||
@doc """
|
||||
|
|
|
@ -9,8 +9,12 @@ defmodule Kinemat.Euler do
|
|||
YZX, ZXY.
|
||||
"""
|
||||
@type valid_representation :: :xyz | :yxz | :zxy | :zyx | :yzx | :xzy
|
||||
@type t :: %Euler{representation: valid_representation,
|
||||
x: Angle.t, y: Angle.t, z: Angle.t}
|
||||
@type t :: %Euler{
|
||||
representation: valid_representation,
|
||||
x: Angle.t(),
|
||||
y: Angle.t(),
|
||||
z: Angle.t()
|
||||
}
|
||||
|
||||
@doc """
|
||||
Initialise a new Euler orientation with angles.
|
||||
|
@ -28,10 +32,9 @@ defmodule Kinemat.Euler do
|
|||
y: ~a(20)d,
|
||||
z: ~a(30)d}
|
||||
"""
|
||||
@spec init(valid_representation, Angle.t, Angle.t, Angle.t) :: t
|
||||
def init(representation, x, y, z)
|
||||
when representation in ~w(xyz yxz zxy zyx yzx xzy)a
|
||||
do
|
||||
@spec init(valid_representation, Angle.t(), Angle.t(), Angle.t()) :: t
|
||||
def init(representation, %Angle{} = x, %Angle{} = y, %Angle{} = z)
|
||||
when representation in ~w(xyz yxz zxy zyx yzx xzy)a do
|
||||
%Euler{representation: representation, x: x, y: y, z: z}
|
||||
end
|
||||
|
||||
|
@ -62,7 +65,7 @@ defmodule Kinemat.Euler do
|
|||
...> |> Euler.x
|
||||
#Angle<13°>
|
||||
"""
|
||||
@spec x(t) :: Angle.t
|
||||
@spec x(t) :: Angle.t()
|
||||
def x(%Euler{x: x}), do: x
|
||||
|
||||
@doc """
|
||||
|
@ -74,7 +77,7 @@ defmodule Kinemat.Euler do
|
|||
...> |> Euler.y
|
||||
#Angle<13°>
|
||||
"""
|
||||
@spec y(t) :: Angle.t
|
||||
@spec y(t) :: Angle.t()
|
||||
def y(%Euler{y: y}), do: y
|
||||
|
||||
@doc """
|
||||
|
@ -86,6 +89,6 @@ defmodule Kinemat.Euler do
|
|||
...> |> Euler.z
|
||||
#Angle<13°>
|
||||
"""
|
||||
@spec z(t) :: Angle.t
|
||||
@spec z(t) :: Angle.t()
|
||||
def z(%Euler{z: z}), do: z
|
||||
end
|
||||
|
|
|
@ -12,7 +12,7 @@ defmodule Kinemat.Euler.ToQuaternion do
|
|||
@doc """
|
||||
Converts Euler orientations into Quaternions.
|
||||
"""
|
||||
@spec to_quaternion(Euler.t) :: Quaternion.t
|
||||
@spec to_quaternion(Euler.t()) :: Quaternion.t()
|
||||
def to_quaternion(%Euler{representation: order, x: x, y: y, z: z}) do
|
||||
{_, x} = Angle.to_radians(x)
|
||||
{_, y} = Angle.to_radians(y)
|
||||
|
|
|
@ -11,7 +11,7 @@ defmodule Kinemat.Euler.ToRotationMatrix do
|
|||
@doc """
|
||||
Convert an Euler orientation into a rotation matrix.
|
||||
"""
|
||||
@spec to_rotation_matrix(Euler.t) :: RotationMatrix.t
|
||||
@spec to_rotation_matrix(Euler.t()) :: RotationMatrix.t()
|
||||
def to_rotation_matrix(%Euler{representation: order, x: x, y: y, z: z}) do
|
||||
a = cos(x)
|
||||
b = sin(x)
|
||||
|
@ -22,37 +22,35 @@ defmodule Kinemat.Euler.ToRotationMatrix do
|
|||
|
||||
order
|
||||
|> build_rotation({a, b, c, d, e, f})
|
||||
|> RotationMatrix.init
|
||||
|> RotationMatrix.init()
|
||||
end
|
||||
|
||||
defp build_rotation(:xyz, {a, b, c, d, e, f}) do
|
||||
ae = a * e
|
||||
af = a * f
|
||||
be = b * e
|
||||
bf = b * f
|
||||
ae = a * e
|
||||
af = a * f
|
||||
be = b * e
|
||||
bf = b * f
|
||||
|
||||
m00 = c * e
|
||||
m10 = - c * f
|
||||
m10 = -c * f
|
||||
m20 = d
|
||||
|
||||
m01 = af + be * d
|
||||
m11 = ae - bf * d
|
||||
m21 = - b * c
|
||||
m21 = -b * c
|
||||
|
||||
m02 = bf - ae * d
|
||||
m12 = be + af * d
|
||||
m22 = a * c
|
||||
|
||||
{m00, m01, m02,
|
||||
m10, m11, m12,
|
||||
m20, m21, m22}
|
||||
{m00, m01, m02, m10, m11, m12, m20, m21, m22}
|
||||
end
|
||||
|
||||
defp build_rotation(:yxz, {a, b, c, d, e, f}) do
|
||||
ce = c * e
|
||||
cf = c * f
|
||||
de = d * e
|
||||
df = d * f
|
||||
ce = c * e
|
||||
cf = c * f
|
||||
de = d * e
|
||||
df = d * f
|
||||
|
||||
m00 = ce + df * b
|
||||
m10 = de * b - cf
|
||||
|
@ -60,45 +58,41 @@ defmodule Kinemat.Euler.ToRotationMatrix do
|
|||
|
||||
m01 = a * f
|
||||
m11 = a * e
|
||||
m21 = - b
|
||||
m21 = -b
|
||||
|
||||
m02 = cf * b - de
|
||||
m12 = df + ce * b
|
||||
m22 = a * c
|
||||
|
||||
{m00, m01, m02,
|
||||
m10, m11, m12,
|
||||
m20, m21, m22}
|
||||
{m00, m01, m02, m10, m11, m12, m20, m21, m22}
|
||||
end
|
||||
|
||||
defp build_rotation(:zxy, {a, b, c, d, e, f}) do
|
||||
ce = c * e
|
||||
cf = c * f
|
||||
de = d * e
|
||||
df = d * f
|
||||
ce = c * e
|
||||
cf = c * f
|
||||
de = d * e
|
||||
df = d * f
|
||||
|
||||
m00 = ce - df * b
|
||||
m10 = - a * f
|
||||
m10 = -a * f
|
||||
m20 = de + cf * b
|
||||
|
||||
m01 = cf + de * b
|
||||
m11 = a * e
|
||||
m21 = df - ce * b
|
||||
|
||||
m02 = - a * d
|
||||
m02 = -a * d
|
||||
m12 = b
|
||||
m22 = a * c
|
||||
|
||||
{m00, m01, m02,
|
||||
m10, m11, m12,
|
||||
m20, m21, m22}
|
||||
{m00, m01, m02, m10, m11, m12, m20, m21, m22}
|
||||
end
|
||||
|
||||
defp build_rotation(:zyx, {a, b, c, d, e, f}) do
|
||||
ae = a * e
|
||||
af = a * f
|
||||
be = b * e
|
||||
bf = b * f
|
||||
ae = a * e
|
||||
af = a * f
|
||||
be = b * e
|
||||
bf = b * f
|
||||
|
||||
m00 = c * e
|
||||
m10 = be * d - af
|
||||
|
@ -108,20 +102,18 @@ defmodule Kinemat.Euler.ToRotationMatrix do
|
|||
m11 = bf * d + ae
|
||||
m21 = af * d - be
|
||||
|
||||
m02 = - d
|
||||
m02 = -d
|
||||
m12 = b * c
|
||||
m22 = a * c
|
||||
|
||||
{m00, m01, m02,
|
||||
m10, m11, m12,
|
||||
m20, m21, m22}
|
||||
{m00, m01, m02, m10, m11, m12, m20, m21, m22}
|
||||
end
|
||||
|
||||
defp build_rotation(:yzx, {a, b, c, d, e, f}) do
|
||||
ac = a * c
|
||||
ad = a * d
|
||||
bc = b * c
|
||||
bd = b * d
|
||||
ac = a * c
|
||||
ad = a * d
|
||||
bc = b * c
|
||||
bd = b * d
|
||||
|
||||
m00 = c * e
|
||||
m10 = bd - ac * f
|
||||
|
@ -129,25 +121,23 @@ defmodule Kinemat.Euler.ToRotationMatrix do
|
|||
|
||||
m01 = f
|
||||
m11 = a * e
|
||||
m21 = - b * e
|
||||
m21 = -b * e
|
||||
|
||||
m02 = - d * e
|
||||
m02 = -d * e
|
||||
m12 = ad * f + bc
|
||||
m22 = ac - bd * f
|
||||
|
||||
{m00, m01, m02,
|
||||
m10, m11, m12,
|
||||
m20, m21, m22}
|
||||
{m00, m01, m02, m10, m11, m12, m20, m21, m22}
|
||||
end
|
||||
|
||||
defp build_rotation(:xzy, {a, b, c, d, e, f}) do
|
||||
ac = a * c
|
||||
ad = a * d
|
||||
bc = b * c
|
||||
bd = b * d
|
||||
ac = a * c
|
||||
ad = a * d
|
||||
bc = b * c
|
||||
bd = b * d
|
||||
|
||||
m00 = c * e
|
||||
m10 = - f
|
||||
m10 = -f
|
||||
m20 = d * e
|
||||
|
||||
m01 = ac * f + bd
|
||||
|
@ -158,9 +148,6 @@ defmodule Kinemat.Euler.ToRotationMatrix do
|
|||
m12 = b * e
|
||||
m22 = bd * f + ac
|
||||
|
||||
{m00, m01, m02,
|
||||
m10, m11, m12,
|
||||
m20, m21, m22}
|
||||
{m00, m01, m02, m10, m11, m12, m20, m21, m22}
|
||||
end
|
||||
|
||||
end
|
||||
|
|
|
@ -7,17 +7,15 @@ defmodule Kinemat.Frame do
|
|||
parent reference frame.
|
||||
"""
|
||||
|
||||
@type t :: %Frame{point: Point.t,
|
||||
orientation: Orientation.t}
|
||||
@type t :: %Frame{point: Point.t(), orientation: Orientation.t()}
|
||||
|
||||
@doc """
|
||||
Initialise a frame from a point and an orientation.
|
||||
"""
|
||||
@spec init(Point.t, Orientation.t) :: t
|
||||
@spec init(Point.t(), Orientation.t()) :: t
|
||||
def init(%{__struct__: p} = point, %{__struct__: o} = orientation)
|
||||
when (p == Cartesian or p == Cylindrical or p == Spherical)
|
||||
and (o == Euler or o == Quaternion or o == RotationMatrix)
|
||||
do
|
||||
when (p == Cartesian or p == Cylindrical or p == Spherical) and
|
||||
(o == Euler or o == Quaternion or o == RotationMatrix) do
|
||||
%Frame{point: point, orientation: orientation}
|
||||
end
|
||||
|
||||
|
@ -30,7 +28,7 @@ defmodule Kinemat.Frame do
|
|||
...> |> Frame.point()
|
||||
#Kinemat.Point<[x: 3, y: 4, z: 5]>
|
||||
"""
|
||||
@spec point(t) :: Point.t
|
||||
@spec point(t) :: Point.t()
|
||||
def point(%Frame{point: p}), do: p
|
||||
|
||||
@doc """
|
||||
|
@ -42,6 +40,6 @@ defmodule Kinemat.Frame do
|
|||
...> |> Frame.orientation()
|
||||
#Kinemat.Orientation<[euler: :xyz, x: #Angle<0>, y: #Angle<0>, z: #Angle<0>]>
|
||||
"""
|
||||
@spec orientation(t) :: Orientation.t
|
||||
@spec orientation(t) :: Orientation.t()
|
||||
def orientation(%Frame{orientation: o}), do: o
|
||||
end
|
||||
|
|
|
@ -5,10 +5,9 @@ defmodule Kinemat.HomogeneousTransformation do
|
|||
Converts a Frame into a 4x4 matrix of it's homogeneous transformation.
|
||||
"""
|
||||
|
||||
@type t :: {number, number, number, number,
|
||||
number, number, number, number,
|
||||
number, number, number, number,
|
||||
number, number, number, number}
|
||||
@type t ::
|
||||
{number, number, number, number, number, number, number, number,
|
||||
number, number, number, number, number, number, number, number}
|
||||
|
||||
@doc """
|
||||
Converts a Frame into it's 4x4 matrix representation as a homogeneous transformation.
|
||||
|
@ -28,28 +27,31 @@ defmodule Kinemat.HomogeneousTransformation do
|
|||
0.0, 0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 0.0, 1.0}
|
||||
"""
|
||||
@spec to_homogeneous_transformation(Frame.t) :: t
|
||||
def to_homogeneous_transformation(%Frame{point: point, orientation: orientation}) do
|
||||
{m00, m01, m02,
|
||||
m10, m11, m12,
|
||||
m20, m21, m22} = matrix_from(orientation)
|
||||
@spec to_homogeneous_transformation(Frame.t()) :: t
|
||||
def to_homogeneous_transformation(%Frame{
|
||||
point: point,
|
||||
orientation: orientation
|
||||
}) do
|
||||
{m00, m01, m02, m10, m11, m12, m20, m21, m22} = matrix_from(orientation)
|
||||
{m03, m13, m23} = coords_from(point)
|
||||
|
||||
{m00, m01, m02, m03,
|
||||
m10, m11, m12, m13,
|
||||
m20, m21, m22, m23,
|
||||
0.0, 0.0, 0.0, 1.0}
|
||||
{m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, 0.0, 0.0, 0.0,
|
||||
1.0}
|
||||
end
|
||||
|
||||
defp coords_from(point) do
|
||||
%{x: x, y: y, z: z} = point
|
||||
%{x: x, y: y, z: z} =
|
||||
point
|
||||
|> Point.to_cartesian()
|
||||
|
||||
{x, y, z}
|
||||
end
|
||||
|
||||
defp matrix_from(orientation) do
|
||||
%{matrix: matrix} = orientation
|
||||
%{matrix: matrix} =
|
||||
orientation
|
||||
|> Orientation.to_rotation_matrix()
|
||||
|
||||
matrix
|
||||
end
|
||||
end
|
||||
|
|
|
@ -1,5 +0,0 @@
|
|||
defprotocol Kinemat.Joint do
|
||||
@moduledoc """
|
||||
FIXME Not yet done.
|
||||
"""
|
||||
end
|
|
@ -6,18 +6,18 @@ defprotocol Kinemat.Orientation do
|
|||
different orientations (Euler, RotationMatrix and Quaternion).
|
||||
"""
|
||||
|
||||
@type t :: Euler.t | RotationMatrix.t | Quaternion.t
|
||||
@type t :: Euler.t() | RotationMatrix.t() | Quaternion.t()
|
||||
|
||||
@doc """
|
||||
Convert the Orientation into it's Rotation Matrix representation.
|
||||
"""
|
||||
@spec to_rotation_matrix(Orientation.t) :: RotationMatrix.t
|
||||
@spec to_rotation_matrix(Orientation.t()) :: RotationMatrix.t()
|
||||
def to_rotation_matrix(orientation)
|
||||
|
||||
@doc """
|
||||
Convert the Orientation into it's Quaternion representation.
|
||||
"""
|
||||
@spec to_quaternion(Orientation.t) :: Quaternion.t
|
||||
@spec to_quaternion(Orientation.t()) :: Quaternion.t()
|
||||
def to_quaternion(orientation)
|
||||
|
||||
@doc """
|
||||
|
@ -25,13 +25,13 @@ defprotocol Kinemat.Orientation do
|
|||
|
||||
Default representation is `:xyz`.
|
||||
"""
|
||||
@spec to_euler(Orientation.t) :: Euler.t
|
||||
@spec to_euler(Orientation.t()) :: Euler.t()
|
||||
def to_euler(orientation)
|
||||
|
||||
@doc """
|
||||
Convert the Orientation into it's Euler angle representation in the
|
||||
specified axis order.
|
||||
"""
|
||||
@spec to_euler(Orientation.t, Euler.valid_representation) :: Euler.t
|
||||
@spec to_euler(Orientation.t(), Euler.valid_representation()) :: Euler.t()
|
||||
def to_euler(orientation, representation)
|
||||
end
|
||||
|
|
|
@ -17,8 +17,9 @@ defimpl Kinemat.Orientation, for: Kinemat.Euler do
|
|||
-0.33036608954935215, 0.7695370178986853, 0.5465080282662533,
|
||||
0.25881904510252074, -0.4829629131445341, 0.8365163037378079}}
|
||||
"""
|
||||
@spec to_rotation_matrix(Euler.t) :: RotationMatrix.t
|
||||
def to_rotation_matrix(euler), do: Euler.ToRotationMatrix.to_rotation_matrix(euler)
|
||||
@spec to_rotation_matrix(Euler.t()) :: RotationMatrix.t()
|
||||
def to_rotation_matrix(euler),
|
||||
do: Euler.ToRotationMatrix.to_rotation_matrix(euler)
|
||||
|
||||
@doc """
|
||||
Convert the euler angle into quaternion.
|
||||
|
@ -32,8 +33,9 @@ defimpl Kinemat.Orientation, for: Kinemat.Euler do
|
|||
%Kinemat.Quaternion{w: ~a(0.9372468582005039)r, x: 0.27459973122432013,
|
||||
y: 0.07960424450132775, z: 0.19956572516889892}
|
||||
"""
|
||||
@spec to_quaternion(Euler.t) :: Quaternion.t
|
||||
def to_quaternion(orientation), do: Euler.ToQuaternion.to_quaternion(orientation)
|
||||
@spec to_quaternion(Euler.t()) :: Quaternion.t()
|
||||
def to_quaternion(orientation),
|
||||
do: Euler.ToQuaternion.to_quaternion(orientation)
|
||||
|
||||
@doc """
|
||||
Convert the Euler from one representation to another.
|
||||
|
@ -49,11 +51,14 @@ defimpl Kinemat.Orientation, for: Kinemat.Euler do
|
|||
y: %Angle{r: -0.2617993877991494},
|
||||
z: %Angle{r: -0.3490658503988659}}
|
||||
"""
|
||||
@spec to_euler(Euler.t) :: Euler.t
|
||||
@spec to_euler(Euler.t()) :: Euler.t()
|
||||
def to_euler(%Euler{} = orientation), do: orientation
|
||||
|
||||
@spec to_euler(Euler.t, Euler.valid_representation) :: Euler.t
|
||||
def to_euler(%Euler{representation: r} = orientation, representation) when r == representation, do: orientation
|
||||
@spec to_euler(Euler.t(), Euler.valid_representation()) :: Euler.t()
|
||||
def to_euler(%Euler{representation: r} = orientation, representation)
|
||||
when r == representation,
|
||||
do: orientation
|
||||
|
||||
def to_euler(orientation, representation) do
|
||||
orientation
|
||||
|> Orientation.to_rotation_matrix()
|
||||
|
|
|
@ -23,9 +23,10 @@ defimpl Kinemat.Orientation, for: Kinemat.Quaternion do
|
|||
}}
|
||||
|
||||
"""
|
||||
@spec to_rotation_matrix(Quaternion.t) :: RotationMatrix.t
|
||||
@spec to_rotation_matrix(Quaternion.t()) :: RotationMatrix.t()
|
||||
def to_rotation_matrix(%Quaternion{w: w, x: x, y: y, z: z}) do
|
||||
{_, r} = Angle.to_radians(w)
|
||||
|
||||
r
|
||||
|> Quatern.create(x, y, z)
|
||||
|> Quatern.to_rotation_matrix()
|
||||
|
@ -35,7 +36,7 @@ defimpl Kinemat.Orientation, for: Kinemat.Quaternion do
|
|||
@doc """
|
||||
Does nothing, simply returns the Quaternion.
|
||||
"""
|
||||
@spec to_quaternion(Quaternion.t) :: Quaternion.t
|
||||
@spec to_quaternion(Quaternion.t()) :: Quaternion.t()
|
||||
def to_quaternion(orientation), do: orientation
|
||||
|
||||
@doc """
|
||||
|
@ -51,7 +52,7 @@ defimpl Kinemat.Orientation, for: Kinemat.Quaternion do
|
|||
y: ~a(0.2945952073784124)r,
|
||||
z: ~a(0.12377521877974283)r}
|
||||
"""
|
||||
@spec to_euler(Quaternion.t, Euler.valid_representation) :: Euler.t
|
||||
@spec to_euler(Quaternion.t(), Euler.valid_representation()) :: Euler.t()
|
||||
def to_euler(orientation, representation \\ :xyz) do
|
||||
orientation
|
||||
|> Orientation.to_rotation_matrix()
|
||||
|
|
|
@ -10,7 +10,7 @@ defimpl Kinemat.Orientation, for: Kinemat.RotationMatrix do
|
|||
@doc """
|
||||
Does nothing, simply returns the rotation matrix.
|
||||
"""
|
||||
@spec to_rotation_matrix(RotationMatrix.t) :: RotationMatrix.t
|
||||
@spec to_rotation_matrix(RotationMatrix.t()) :: RotationMatrix.t()
|
||||
def to_rotation_matrix(orientation), do: orientation
|
||||
|
||||
@doc """
|
||||
|
@ -24,10 +24,12 @@ defimpl Kinemat.Orientation, for: Kinemat.RotationMatrix do
|
|||
...> |> Orientation.to_quaternion()
|
||||
%Kinemat.Quaternion{w: ~a(0.7071067811865476)r, x: 0.7071067811865475, y: 0.0, z: 0.0}
|
||||
"""
|
||||
@spec to_quaternion(RotationMatrix.t) :: Quaternion.t
|
||||
@spec to_quaternion(RotationMatrix.t()) :: Quaternion.t()
|
||||
def to_quaternion(%RotationMatrix{matrix: matrix}) do
|
||||
{w, x, y, z} = matrix
|
||||
{w, x, y, z} =
|
||||
matrix
|
||||
|> Quatern.from_rotation_matrix()
|
||||
|
||||
Quaternion.init(Radian.init(w), x, y, z)
|
||||
end
|
||||
|
||||
|
@ -41,13 +43,15 @@ defimpl Kinemat.Orientation, for: Kinemat.RotationMatrix do
|
|||
...> |> RotationMatrix.init()
|
||||
...> |> Orientation.to_euler()
|
||||
%Kinemat.Euler{representation: :xyz,
|
||||
x: %Angle{r: 3.141592653589793},
|
||||
y: %Angle{r: 0.0},
|
||||
z: %Angle{r: -1.5707963267948966}}
|
||||
x: ~a(3.141592653589793)r,
|
||||
y: ~a(0),
|
||||
z: ~a(-1.5707963267948966)r}
|
||||
"""
|
||||
@spec to_euler(RotationMatrix.t) :: Euler.t
|
||||
def to_euler(orientation), do: RotationMatrix.ToEuler.to_euler(orientation, :xyz)
|
||||
@spec to_euler(RotationMatrix.t()) :: Euler.t()
|
||||
def to_euler(orientation),
|
||||
do: RotationMatrix.ToEuler.to_euler(orientation, :xyz)
|
||||
|
||||
@spec to_euler(RotationMatrix.t, Euler.valid_orientation) :: Euler.t
|
||||
def to_euler(orientation, representation), do: RotationMatrix.ToEuler.to_euler(orientation, representation)
|
||||
@spec to_euler(RotationMatrix.t(), Euler.valid_orientation()) :: Euler.t()
|
||||
def to_euler(orientation, representation),
|
||||
do: RotationMatrix.ToEuler.to_euler(orientation, representation)
|
||||
end
|
||||
|
|
|
@ -5,23 +5,23 @@ defprotocol Kinemat.Point do
|
|||
Describes a Point in 3 dimensional space.
|
||||
"""
|
||||
|
||||
@type t :: Cartesian.t | Cylindrical.t | Spherical.t
|
||||
@type t :: Cartesian.t() | Cylindrical.t() | Spherical.t()
|
||||
|
||||
@doc """
|
||||
Convert the Point into it's cartesian representation.
|
||||
"""
|
||||
@spec to_cartesian(Point.t) :: Cartesian.t
|
||||
@spec to_cartesian(Point.t()) :: Cartesian.t()
|
||||
def to_cartesian(point)
|
||||
|
||||
@doc """
|
||||
Convert the Point into it's cylindrical representation.
|
||||
"""
|
||||
@spec to_cylindrical(Point.t) :: Cylindrical.t
|
||||
@spec to_cylindrical(Point.t()) :: Cylindrical.t()
|
||||
def to_cylindrical(point)
|
||||
|
||||
@doc """
|
||||
Convert the Point into it's spherical representation.
|
||||
"""
|
||||
@spec to_spherical(Point.t) :: Spherical.t
|
||||
@spec to_spherical(Point.t()) :: Spherical.t()
|
||||
def to_spherical(point)
|
||||
end
|
||||
|
|
|
@ -12,7 +12,7 @@ defimpl Kinemat.Point, for: Kinemat.Cartesian do
|
|||
...> |> Kinemat.Point.to_cartesian()
|
||||
%Kinemat.Cartesian{x: 1, y: 2, z: 3}
|
||||
"""
|
||||
@spec to_cartesian(Cartesian.t) :: Cartesian.t
|
||||
@spec to_cartesian(Cartesian.t()) :: Cartesian.t()
|
||||
def to_cartesian(%Cartesian{} = point), do: point
|
||||
|
||||
@doc """
|
||||
|
@ -27,9 +27,9 @@ defimpl Kinemat.Point, for: Kinemat.Cartesian do
|
|||
azimuth: %Angle{r: 0.9272952180016122},
|
||||
vertical: 7}
|
||||
"""
|
||||
@spec to_cylindrical(Cartesian.t) :: Cylindrical.t
|
||||
@spec to_cylindrical(Cartesian.t()) :: Cylindrical.t()
|
||||
def to_cylindrical(%Cartesian{x: x, y: y, z: z}) do
|
||||
r = sqrt(pow(x, 2) + pow(y, 2))
|
||||
r = sqrt(pow(x, 2) + pow(y, 2))
|
||||
theta = Radian.init(atan(y / x))
|
||||
Cylindrical.init(r, theta, z)
|
||||
end
|
||||
|
@ -46,11 +46,11 @@ defimpl Kinemat.Point, for: Kinemat.Cartesian do
|
|||
azimuth: %Angle{r: 0.4234308319224211},
|
||||
polar: %Angle{r: 0.982793723247329}}
|
||||
"""
|
||||
@spec to_spherical(Cartesian.t) :: Sperical.t
|
||||
@spec to_spherical(Cartesian.t()) :: Sperical.t()
|
||||
def to_spherical(%Cartesian{x: x, y: y, z: z}) do
|
||||
rho = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))
|
||||
rho = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))
|
||||
theta = Radian.init(acos(z / rho))
|
||||
phi = Radian.init(atan(y / x))
|
||||
phi = Radian.init(atan(y / x))
|
||||
Spherical.init(rho, theta, phi)
|
||||
end
|
||||
end
|
||||
|
|
|
@ -20,7 +20,7 @@ defimpl Kinemat.Point, for: Kinemat.Cylindrical do
|
|||
radial: 3,
|
||||
vertical: 9}
|
||||
"""
|
||||
@spec to_cylindrical(Cylindrical.t) :: Cylindrical.t
|
||||
@spec to_cylindrical(Cylindrical.t()) :: Cylindrical.t()
|
||||
def to_cylindrical(point), do: point
|
||||
|
||||
@doc """
|
||||
|
@ -32,7 +32,7 @@ defimpl Kinemat.Point, for: Kinemat.Cylindrical do
|
|||
...> |> Point.to_cartesian()
|
||||
%Kinemat.Cartesian{x: 2.954423259036624, y: 0.520944533000791, z: 9}
|
||||
"""
|
||||
@spec to_cartesian(Cylindrical.t) :: Cartesian.t
|
||||
@spec to_cartesian(Cylindrical.t()) :: Cartesian.t()
|
||||
def to_cartesian(%Cylindrical{radial: r, azimuth: theta, vertical: z}) do
|
||||
x = cos(theta)
|
||||
y = sin(theta)
|
||||
|
@ -51,10 +51,10 @@ defimpl Kinemat.Point, for: Kinemat.Cylindrical do
|
|||
polar: ~a(20)d,
|
||||
radial: 5.0}
|
||||
"""
|
||||
@spec to_spherical(Cylindrical.t) :: Spherical.t
|
||||
@spec to_spherical(Cylindrical.t()) :: Spherical.t()
|
||||
def to_spherical(%Cylindrical{radial: r0, azimuth: theta, vertical: z}) do
|
||||
r1 = sqrt(pow(r0, 2) + pow(z, 2))
|
||||
phi = theta
|
||||
r1 = sqrt(pow(r0, 2) + pow(z, 2))
|
||||
phi = theta
|
||||
theta = Radian.init(atan(r0 / z))
|
||||
Spherical.init(r1, theta, phi)
|
||||
end
|
||||
|
|
|
@ -18,13 +18,13 @@ defimpl Kinemat.Point, for: Kinemat.Spherical do
|
|||
radial: 3.9999999999999996,
|
||||
vertical: 6.92820323027551}
|
||||
"""
|
||||
@spec to_cylindrical(Spherical.t) :: Cylindrical.t
|
||||
@spec to_cylindrical(Spherical.t()) :: Cylindrical.t()
|
||||
def to_cylindrical(%Spherical{radial: r, azimuth: theta, polar: phi}) do
|
||||
theta1 = theta
|
||||
theta = phi
|
||||
theta = phi
|
||||
|
||||
r1 = r * sin(theta1)
|
||||
z = r * cos(theta1)
|
||||
r1 = r * sin(theta1)
|
||||
z = r * cos(theta1)
|
||||
Cylindrical.init(r1, theta, z)
|
||||
end
|
||||
|
||||
|
@ -40,11 +40,11 @@ defimpl Kinemat.Point, for: Kinemat.Spherical do
|
|||
y: 3.554377592712286,
|
||||
z: 11.276311449430901}
|
||||
"""
|
||||
@spec to_cartesian(Spherical.t) :: Cartesian.t
|
||||
@spec to_cartesian(Spherical.t()) :: Cartesian.t()
|
||||
def to_cartesian(%Spherical{radial: r, azimuth: theta, polar: phi}) do
|
||||
x = r * sin(theta) * cos(phi)
|
||||
y = r * sin(theta) * sin(phi)
|
||||
z = r * cos(theta)
|
||||
x = r * sin(theta) * cos(phi)
|
||||
y = r * sin(theta) * sin(phi)
|
||||
z = r * cos(theta)
|
||||
Cartesian.init(x, y, z)
|
||||
end
|
||||
|
||||
|
@ -60,6 +60,6 @@ defimpl Kinemat.Point, for: Kinemat.Spherical do
|
|||
polar: ~a(60)d,
|
||||
radial: 12}
|
||||
"""
|
||||
@spec to_spherical(Spherical.t) :: Spherical.t
|
||||
@spec to_spherical(Spherical.t()) :: Spherical.t()
|
||||
def to_spherical(point), do: point
|
||||
end
|
||||
|
|
|
@ -6,10 +6,7 @@ defmodule Kinemat.Quaternion do
|
|||
Describes an orientation using Quaternions.
|
||||
"""
|
||||
|
||||
@type t :: %Quaternion{w: Angle.t,
|
||||
x: number,
|
||||
y: number,
|
||||
z: number}
|
||||
@type t :: %Quaternion{w: Angle.t(), x: number, y: number, z: number}
|
||||
|
||||
@doc """
|
||||
Create a Quaternion using `w`, `x`, `y` and `z` where
|
||||
|
@ -22,13 +19,13 @@ defmodule Kinemat.Quaternion do
|
|||
iex> Quaternion.init(~a(90)d, 10, 20, 30)
|
||||
%Kinemat.Quaternion{w: ~a(90)d, x: 10, y: 20, z: 30}
|
||||
"""
|
||||
@spec init(Angle.t, number, number, number) :: t
|
||||
@spec init(Angle.t(), number, number, number) :: t
|
||||
def init(%Angle{} = w, x, y, z), do: %Quaternion{w: w, x: x, y: y, z: z}
|
||||
|
||||
@doc """
|
||||
Returns the `w` component of the Quaternion.
|
||||
"""
|
||||
@spec w(t) :: Angle.t
|
||||
@spec w(t) :: Angle.t()
|
||||
def w(%Quaternion{w: w}), do: w
|
||||
|
||||
@doc """
|
||||
|
|
|
@ -6,9 +6,9 @@ defmodule Kinemat.RotationMatrix do
|
|||
Describes a 3 dimension orientation with a Rotation Matrix.
|
||||
"""
|
||||
|
||||
@type matrix3 :: {number, number, number,
|
||||
number, number, number,
|
||||
number, number, number}
|
||||
@type matrix3 ::
|
||||
{number, number, number, number, number, number, number, number,
|
||||
number}
|
||||
@type t :: %RotationMatrix{matrix: matrix3}
|
||||
|
||||
@doc """
|
||||
|
@ -22,10 +22,9 @@ defmodule Kinemat.RotationMatrix do
|
|||
"""
|
||||
@spec init(matrix3) :: t
|
||||
def init({n0, n1, n2, n3, n4, n5, n6, n7, n8} = matrix)
|
||||
when is_number(n0) and is_number(n1) and is_number(n2)
|
||||
and is_number(n3) and is_number(n4) and is_number(n5)
|
||||
and is_number(n6) and is_number(n7) and is_number(n8)
|
||||
do
|
||||
when is_number(n0) and is_number(n1) and is_number(n2) and is_number(n3) and
|
||||
is_number(n4) and is_number(n5) and is_number(n6) and is_number(n7) and
|
||||
is_number(n8) do
|
||||
%RotationMatrix{matrix: matrix}
|
||||
end
|
||||
|
||||
|
|
|
@ -12,24 +12,20 @@ defmodule Kinemat.RotationMatrix.ToEuler do
|
|||
@doc """
|
||||
Convert a RotationMatrix into an :xyz Euler.
|
||||
"""
|
||||
@spec to_euler(RotationMatrix.t) :: Euler.t
|
||||
@spec to_euler(RotationMatrix.t()) :: Euler.t()
|
||||
def to_euler(orientation), do: to_euler(orientation, :xyz)
|
||||
|
||||
@doc """
|
||||
Convert a RotationMatrix into a Euler of the specified order.
|
||||
"""
|
||||
@spec to_euler(RotationMatrix.t, Euler.valid_orientation) :: Euler.t
|
||||
@spec to_euler(RotationMatrix.t(), Euler.valid_orientation()) :: Euler.t()
|
||||
def to_euler(%RotationMatrix{matrix: matrix}, representation) do
|
||||
{x, y, z} = compute_euler(matrix, representation)
|
||||
Euler.init(representation,
|
||||
Radian.init(x),
|
||||
Radian.init(y),
|
||||
Radian.init(z))
|
||||
Euler.init(representation, Radian.init(x), Radian.init(y), Radian.init(z))
|
||||
end
|
||||
|
||||
defp compute_euler({m00, m01, m02, _, _, m12, _, _, m22}, :xyz)
|
||||
when abs(m02) < 0.99999
|
||||
do
|
||||
when abs(m02) < 0.99999 do
|
||||
y = m02 |> clamp |> asin()
|
||||
x = atan2(0 - m12, m22)
|
||||
z = atan2(0 - m01, m00)
|
||||
|
@ -44,7 +40,7 @@ defmodule Kinemat.RotationMatrix.ToEuler do
|
|||
end
|
||||
|
||||
defp compute_euler({_, _, m02, m10, m11, m12, _, _, m22}, :yxz)
|
||||
when abs(m12) < 0.99999 do
|
||||
when abs(m12) < 0.99999 do
|
||||
x = asin(0 - clamp(m12))
|
||||
y = atan2(m02, m22)
|
||||
z = atan2(m10, m11)
|
||||
|
@ -59,7 +55,7 @@ defmodule Kinemat.RotationMatrix.ToEuler do
|
|||
end
|
||||
|
||||
defp compute_euler({_, m01, _, _, m11, _, m20, m21, m22}, :zxy)
|
||||
when abs(m21) < 0.99999 do
|
||||
when abs(m21) < 0.99999 do
|
||||
x = m21 |> clamp |> asin()
|
||||
y = atan2(0 - m20, m22)
|
||||
z = atan2(0 - m01, m11)
|
||||
|
@ -74,7 +70,7 @@ defmodule Kinemat.RotationMatrix.ToEuler do
|
|||
end
|
||||
|
||||
defp compute_euler({m00, _, _, m10, _, _, m20, m21, m22}, :zyx)
|
||||
when abs(m20) < 0.99999 do
|
||||
when abs(m20) < 0.99999 do
|
||||
y = asin(0 - clamp(m20))
|
||||
x = atan2(m21, m22)
|
||||
z = atan2(m10, m00)
|
||||
|
@ -89,7 +85,7 @@ defmodule Kinemat.RotationMatrix.ToEuler do
|
|||
end
|
||||
|
||||
defp compute_euler({m00, _, _, m10, m11, m12, m20, _, _}, :yzx)
|
||||
when abs(m10) < 0.99999 do
|
||||
when abs(m10) < 0.99999 do
|
||||
z = m10 |> clamp |> asin()
|
||||
x = atan2(0 - m12, m11)
|
||||
y = atan2(0 - m20, m00)
|
||||
|
@ -104,7 +100,7 @@ defmodule Kinemat.RotationMatrix.ToEuler do
|
|||
end
|
||||
|
||||
defp compute_euler({m00, m01, m02, _, m11, _, _, m21, _}, :xzy)
|
||||
when abs(m01) < 0.99999 do
|
||||
when abs(m01) < 0.99999 do
|
||||
z = asin(0 - clamp(m01))
|
||||
x = atan2(m21, m11)
|
||||
y = atan2(m02, m00)
|
||||
|
@ -121,5 +117,4 @@ defmodule Kinemat.RotationMatrix.ToEuler do
|
|||
defp clamp(value) when value < -1, do: -1
|
||||
defp clamp(value) when value > 1, do: 1
|
||||
defp clamp(value), do: value
|
||||
|
||||
end
|
||||
|
|
|
@ -12,16 +12,17 @@ defmodule Kinemat.RotationMatrix.ToQuaternion do
|
|||
Converts rotation matrices into quaternions.
|
||||
Used internally by the `Orientation` implementation.
|
||||
"""
|
||||
@spec to_quaternion(RotationMatrix.t) :: Quaternion.t
|
||||
def to_quaternion(%RotationMatrix{matrix: {m00, _, _, _, m11, _, _, _, m22} = matrix}) do
|
||||
trace = m00 + m11 + m22
|
||||
@spec to_quaternion(RotationMatrix.t()) :: Quaternion.t()
|
||||
def to_quaternion(%RotationMatrix{
|
||||
matrix: {m00, _, _, _, m11, _, _, _, m22} = matrix
|
||||
}) do
|
||||
trace = m00 + m11 + m22
|
||||
{w, x, y, z} = build_quaternion(matrix, trace)
|
||||
Quaternion.init(Radian.init(w), x, y, z)
|
||||
end
|
||||
|
||||
defp build_quaternion({_, m01, m02, m10, _, m12, m20, m21, _}, trace)
|
||||
when trace > 0
|
||||
do
|
||||
when trace > 0 do
|
||||
s = 0.5 / sqrt(trace + 1.0)
|
||||
w = 0.25 / 2
|
||||
x = (m21 - m12) * s
|
||||
|
@ -31,8 +32,7 @@ defmodule Kinemat.RotationMatrix.ToQuaternion do
|
|||
end
|
||||
|
||||
defp build_quaternion({m00, m01, m02, m10, m11, m12, m20, m21, m22}, _trace)
|
||||
when m00 > m11 and m00 > m22
|
||||
do
|
||||
when m00 > m11 and m00 > m22 do
|
||||
s = 2.0 * sqrt(1.0 + m00 - m11 - m22)
|
||||
w = (m21 - m12) / s
|
||||
x = 0.25 * s
|
||||
|
@ -42,8 +42,7 @@ defmodule Kinemat.RotationMatrix.ToQuaternion do
|
|||
end
|
||||
|
||||
defp build_quaternion({m00, m01, m02, m10, m11, m12, m20, m21, m22}, _trace)
|
||||
when m11 > m22
|
||||
do
|
||||
when m11 > m22 do
|
||||
s = 2.0 * sqrt(1.0 + m11 - m00 - m22)
|
||||
w = (m02 - m20) / s
|
||||
x = (m01 + m10) / s
|
||||
|
|
|
@ -6,21 +6,19 @@ defmodule Kinemat.Spherical do
|
|||
Describes a point in 3D space using sperical notation (r,θ,ɸ).
|
||||
"""
|
||||
|
||||
@type t :: %Spherical{radial: number,
|
||||
azimuth: Angle.t,
|
||||
polar: Angle.t}
|
||||
@type t :: %Spherical{radial: number, azimuth: Angle.t(), polar: Angle.t()}
|
||||
|
||||
@doc """
|
||||
Initialise a sperical coordinate point from `rho`, `theta` and `phi` (r,θ,ɸ).
|
||||
|
||||
## Examples
|
||||
|
||||
iex> Spherical.init(10, Degrees.init(20), Degrees.init(30))
|
||||
iex> Spherical.init(10, ~a(20)d, ~a(30)d)
|
||||
%Kinemat.Spherical{radial: 10,
|
||||
azimuth: ~a(20)d,
|
||||
polar: ~a(30)d}
|
||||
"""
|
||||
@spec init(number, Angle.t, Angle.t) :: t
|
||||
@spec init(number, Angle.t(), Angle.t()) :: t
|
||||
def init(rho, %Angle{} = theta, %Angle{} = phi) do
|
||||
%Spherical{radial: rho, azimuth: theta, polar: phi}
|
||||
end
|
||||
|
@ -46,25 +44,25 @@ defmodule Kinemat.Spherical do
|
|||
@doc """
|
||||
Alias for `azimuth/1`.
|
||||
"""
|
||||
@spec theta(t) :: Angle.t
|
||||
@spec theta(t) :: Angle.t()
|
||||
def theta(point), do: point |> azimuth()
|
||||
|
||||
@doc """
|
||||
Return the azimuthal component of the point.
|
||||
Return the azimuthal component of the point.
|
||||
|
||||
## Examples
|
||||
## Examples
|
||||
|
||||
iex> Spherical.init(10, ~a(20)d, ~a(30)d)
|
||||
...> |> Spherical.azimuth
|
||||
~a(20)d
|
||||
"""
|
||||
@spec azimuth(t) :: Angle.t
|
||||
iex> Spherical.init(10, ~a(20)d, ~a(30)d)
|
||||
...> |> Spherical.azimuth
|
||||
~a(20)d
|
||||
"""
|
||||
@spec azimuth(t) :: Angle.t()
|
||||
def azimuth(%Spherical{azimuth: theta}), do: theta
|
||||
|
||||
@doc """
|
||||
Alias for `polar/1`.
|
||||
"""
|
||||
@spec phi(t) :: Angle.t
|
||||
@spec phi(t) :: Angle.t()
|
||||
def phi(point), do: point |> polar()
|
||||
|
||||
@doc """
|
||||
|
@ -76,6 +74,6 @@ defmodule Kinemat.Spherical do
|
|||
...> |> Spherical.polar
|
||||
~a(30)d
|
||||
"""
|
||||
@spec polar(t) :: Angle.t
|
||||
@spec polar(t) :: Angle.t()
|
||||
def polar(%Spherical{polar: phi}), do: phi
|
||||
end
|
||||
|
|
|
@ -1,4 +1,8 @@
|
|||
defmodule Kinemat.Trig do
|
||||
@moduledoc """
|
||||
A simple wrapper around `Angle`'s trig functions for compatibility.
|
||||
"""
|
||||
|
||||
def sin(%Angle{} = angle) do
|
||||
{_, result} = Angle.sin(angle)
|
||||
result
|
||||
|
|
9
mix.exs
9
mix.exs
|
@ -11,7 +11,7 @@ defmodule Kinemat.Mixfile do
|
|||
app: :kinemat,
|
||||
version: @version,
|
||||
elixir: "~> 1.5",
|
||||
start_permanent: Mix.env == :prod,
|
||||
start_permanent: Mix.env() == :prod,
|
||||
package: package(),
|
||||
deps: deps(),
|
||||
description: @description
|
||||
|
@ -20,8 +20,8 @@ defmodule Kinemat.Mixfile do
|
|||
|
||||
def package do
|
||||
[
|
||||
maintainers: [ "James Harton <james@automat.nz>" ],
|
||||
licenses: [ "MIT" ],
|
||||
maintainers: ["James Harton <james@automat.nz>"],
|
||||
licenses: ["MIT"],
|
||||
links: %{
|
||||
"Source" => "https://gitlab.com/jimsy/kinemat"
|
||||
}
|
||||
|
@ -43,9 +43,8 @@ defmodule Kinemat.Mixfile do
|
|||
{:credo, "~> 0.6", only: ~w(dev test)a, runtime: false},
|
||||
{:inch_ex, "~> 0.5", only: ~w(dev test)a, runtime: false},
|
||||
{:dialyxir, "~> 0.5", only: ~w(dev test)a, runtime: false},
|
||||
|
||||
{:graphmath, "~> 1.0"},
|
||||
{:angle, "~> 0.2"}
|
||||
{:angle, ">= 0.2.1"}
|
||||
]
|
||||
end
|
||||
end
|
||||
|
|
20
mix.lock
20
mix.lock
|
@ -1,9 +1,11 @@
|
|||
%{"angle": {:hex, :angle, "0.2.0", "2295005054c10dd9841ce657924de75109e1a9b364cc771c406b6d22f2693745", [], [], "hexpm"},
|
||||
"bunt": {:hex, :bunt, "0.2.0", "951c6e801e8b1d2cbe58ebbd3e616a869061ddadcc4863d0a2182541acae9a38", [], [], "hexpm"},
|
||||
"credo": {:hex, :credo, "0.8.8", "990e7844a8d06ebacd88744a55853a83b74270b8a8461c55a4d0334b8e1736c9", [], [{:bunt, "~> 0.2.0", [hex: :bunt, repo: "hexpm", optional: false]}], "hexpm"},
|
||||
"dialyxir": {:hex, :dialyxir, "0.5.1", "b331b091720fd93e878137add264bac4f644e1ddae07a70bf7062c7862c4b952", [], [], "hexpm"},
|
||||
"earmark": {:hex, :earmark, "1.2.3", "206eb2e2ac1a794aa5256f3982de7a76bf4579ff91cb28d0e17ea2c9491e46a4", [], [], "hexpm"},
|
||||
"ex_doc": {:hex, :ex_doc, "0.18.1", "37c69d2ef62f24928c1f4fdc7c724ea04aecfdf500c4329185f8e3649c915baf", [], [{:earmark, "~> 1.1", [hex: :earmark, repo: "hexpm", optional: false]}], "hexpm"},
|
||||
"graphmath": {:hex, :graphmath, "1.0.3", "ff961a5533fea4648ae47501ff241b4004ee719220d251abd44bc83f78f891bb", [], [], "hexpm"},
|
||||
"inch_ex": {:hex, :inch_ex, "0.5.6", "418357418a553baa6d04eccd1b44171936817db61f4c0840112b420b8e378e67", [], [{:poison, "~> 1.5 or ~> 2.0 or ~> 3.0", [hex: :poison, repo: "hexpm", optional: false]}], "hexpm"},
|
||||
"poison": {:hex, :poison, "3.1.0", "d9eb636610e096f86f25d9a46f35a9facac35609a7591b3be3326e99a0484665", [], [], "hexpm"}}
|
||||
%{
|
||||
"angle": {:hex, :angle, "0.2.1", "b2b5df5053b52a2fa65c9604a34570fda18a82c44fd3c7f9e83ce5acf881b4bf", [:mix], [], "hexpm"},
|
||||
"bunt": {:hex, :bunt, "0.2.0", "951c6e801e8b1d2cbe58ebbd3e616a869061ddadcc4863d0a2182541acae9a38", [:mix], [], "hexpm"},
|
||||
"credo": {:hex, :credo, "0.8.10", "261862bb7363247762e1063713bb85df2bbd84af8d8610d1272cd9c1943bba63", [:mix], [{:bunt, "~> 0.2.0", [hex: :bunt, repo: "hexpm", optional: false]}], "hexpm"},
|
||||
"dialyxir": {:hex, :dialyxir, "0.5.1", "b331b091720fd93e878137add264bac4f644e1ddae07a70bf7062c7862c4b952", [:mix], [], "hexpm"},
|
||||
"earmark": {:hex, :earmark, "1.2.4", "99b637c62a4d65a20a9fb674b8cffb8baa771c04605a80c911c4418c69b75439", [:mix], [], "hexpm"},
|
||||
"ex_doc": {:hex, :ex_doc, "0.18.1", "37c69d2ef62f24928c1f4fdc7c724ea04aecfdf500c4329185f8e3649c915baf", [:mix], [{:earmark, "~> 1.1", [hex: :earmark, repo: "hexpm", optional: false]}], "hexpm"},
|
||||
"graphmath": {:hex, :graphmath, "1.0.3", "ff961a5533fea4648ae47501ff241b4004ee719220d251abd44bc83f78f891bb", [:mix], [], "hexpm"},
|
||||
"inch_ex": {:hex, :inch_ex, "0.5.6", "418357418a553baa6d04eccd1b44171936817db61f4c0840112b420b8e378e67", [:mix], [{:poison, "~> 1.5 or ~> 2.0 or ~> 3.0", [hex: :poison, repo: "hexpm", optional: false]}], "hexpm"},
|
||||
"poison": {:hex, :poison, "3.1.0", "d9eb636610e096f86f25d9a46f35a9facac35609a7591b3be3326e99a0484665", [:mix], [], "hexpm"},
|
||||
}
|
||||
|
|
|
@ -11,19 +11,39 @@ defmodule Kinemat.Orientation.EulerToQuaternionTest do
|
|||
|
||||
# These values computed using the Mittsu Ruby gem.
|
||||
@computed %{
|
||||
xyz: {0.9372468582005039, 0.2745997312243201, 0.13302686547026613, 0.168722160571667},
|
||||
yxz: {0.9489794544309479, 0.2745997312243201, 0.13302686547026613, 0.07960424450132775},
|
||||
zxy: {0.9372468582005039, 0.2308130859876117, 0.1995657251688989, 0.168722160571667},
|
||||
zyx: {0.9489794544309479, 0.2308130859876117, 0.1995657251688989, 0.07960424450132775},
|
||||
yzx: {0.9372468582005039, 0.2745997312243201, 0.1995657251688989, 0.07960424450132775},
|
||||
xzy: {0.9489794544309479, 0.2308130859876117, 0.13302686547026613, 0.168722160571667}
|
||||
xyz:
|
||||
{0.9372468582005039, 0.2745997312243201, 0.13302686547026613,
|
||||
0.168722160571667},
|
||||
yxz:
|
||||
{0.9489794544309479, 0.2745997312243201, 0.13302686547026613,
|
||||
0.07960424450132775},
|
||||
zxy:
|
||||
{0.9372468582005039, 0.2308130859876117, 0.1995657251688989,
|
||||
0.168722160571667},
|
||||
zyx:
|
||||
{0.9489794544309479, 0.2308130859876117, 0.1995657251688989,
|
||||
0.07960424450132775},
|
||||
yzx:
|
||||
{0.9372468582005039, 0.2745997312243201, 0.1995657251688989,
|
||||
0.07960424450132775},
|
||||
xzy:
|
||||
{0.9489794544309479, 0.2308130859876117, 0.13302686547026613,
|
||||
0.168722160571667}
|
||||
}
|
||||
|
||||
Enum.each(@computed, fn {representation, {w, x, y, z}} ->
|
||||
test "convert #{representation} Euler into Quaternion" do
|
||||
euler = Euler.init(unquote(representation), @deg_30, @deg_20, @deg_15)
|
||||
q0 = Quaternion.init(Radian.init(unquote(w)), unquote(x), unquote(y), unquote(z))
|
||||
q1 = to_quaternion(euler)
|
||||
|
||||
q0 =
|
||||
Quaternion.init(
|
||||
Radian.init(unquote(w)),
|
||||
unquote(x),
|
||||
unquote(y),
|
||||
unquote(z)
|
||||
)
|
||||
|
||||
q1 = to_quaternion(euler)
|
||||
assert q0 == q1
|
||||
end
|
||||
end)
|
||||
|
|
|
@ -10,36 +10,44 @@ defmodule Kinemat.Orientation.EulerToRotationMatrixTest do
|
|||
|
||||
# These results were computed using the Mittsu ruby gem.
|
||||
@computed_results %{
|
||||
xyz: { 0.9076733711903687, 0.3893269128166894, -0.15669590354740334,
|
||||
-0.24321034680169396, 0.7922556402871195, 0.5596246310178333,
|
||||
0.3420201433256687, -0.46984631039295416, 0.8137976813493738},
|
||||
yxz: { 0.9519340346410572, 0.2241438680420134, -0.20876091614850517,
|
||||
-0.07802730202701791, 0.8365163037378079, 0.5423580124965611,
|
||||
0.29619813272602386, -0.49999999999999994, 0.8137976813493738},
|
||||
zxy: { 0.8634127077396803, 0.40839339157637, -0.29619813272602386,
|
||||
-0.2241438680420134, 0.8365163037378079, 0.49999999999999994,
|
||||
0.4519712629501991, -0.36531535869380743, 0.8137976813493738},
|
||||
zyx: { 0.9076733711903687, 0.24321034680169396, -0.3420201433256687,
|
||||
-0.05896082326733734, 0.8807769671884964, 0.46984631039295416,
|
||||
0.415514948649924, -0.40630119527123487, 0.8137976813493738},
|
||||
yzx: { 0.9076733711903687, 0.25881904510252074, -0.33036608954935215,
|
||||
-0.03961626713065605, 0.8365163037378079, 0.5465080282662533,
|
||||
0.41780330612687083, -0.4829629131445341, 0.7695370178986853},
|
||||
xzy: { 0.9076733711903687, 0.3816364104563247, -0.17459295932517688,
|
||||
-0.25881904510252074, 0.8365163037378079, 0.4829629131445341,
|
||||
0.33036608954935215, -0.393184592519655, 0.8580583448000623}
|
||||
xyz:
|
||||
{0.9076733711903687, 0.3893269128166894, -0.15669590354740334,
|
||||
-0.24321034680169396, 0.7922556402871195, 0.5596246310178333,
|
||||
0.3420201433256687, -0.46984631039295416, 0.8137976813493738},
|
||||
yxz:
|
||||
{0.9519340346410572, 0.2241438680420134, -0.20876091614850517,
|
||||
-0.07802730202701791, 0.8365163037378079, 0.5423580124965611,
|
||||
0.29619813272602386, -0.49999999999999994, 0.8137976813493738},
|
||||
zxy:
|
||||
{0.8634127077396803, 0.40839339157637, -0.29619813272602386,
|
||||
-0.2241438680420134, 0.8365163037378079, 0.49999999999999994,
|
||||
0.4519712629501991, -0.36531535869380743, 0.8137976813493738},
|
||||
zyx:
|
||||
{0.9076733711903687, 0.24321034680169396, -0.3420201433256687,
|
||||
-0.05896082326733734, 0.8807769671884964, 0.46984631039295416,
|
||||
0.415514948649924, -0.40630119527123487, 0.8137976813493738},
|
||||
yzx:
|
||||
{0.9076733711903687, 0.25881904510252074, -0.33036608954935215,
|
||||
-0.03961626713065605, 0.8365163037378079, 0.5465080282662533,
|
||||
0.41780330612687083, -0.4829629131445341, 0.7695370178986853},
|
||||
xzy:
|
||||
{0.9076733711903687, 0.3816364104563247, -0.17459295932517688,
|
||||
-0.25881904510252074, 0.8365163037378079, 0.4829629131445341,
|
||||
0.33036608954935215, -0.393184592519655, 0.8580583448000623}
|
||||
}
|
||||
|
||||
Enum.each(@computed_results, fn
|
||||
{orientation, {m00, m01, m02, m10, m11, m12, m20, m21, m22}} ->
|
||||
test "converting from #{orientation} Euler to rotation matrix" do
|
||||
euler = Euler.init(unquote(orientation), @deg_30, @deg_20, @deg_15)
|
||||
computed = to_rotation_matrix(euler)
|
||||
correct = {unquote(m00), unquote(m01), unquote(m02),
|
||||
unquote(m10), unquote(m11), unquote(m12),
|
||||
unquote(m20), unquote(m21), unquote(m22)}
|
||||
|> RotationMatrix.init()
|
||||
assert computed == correct
|
||||
end
|
||||
Enum.each(@computed_results, fn {orientation,
|
||||
{m00, m01, m02, m10, m11, m12, m20, m21, m22}} ->
|
||||
test "converting from #{orientation} Euler to rotation matrix" do
|
||||
euler = Euler.init(unquote(orientation), @deg_30, @deg_20, @deg_15)
|
||||
computed = to_rotation_matrix(euler)
|
||||
|
||||
correct =
|
||||
{unquote(m00), unquote(m01), unquote(m02), unquote(m10), unquote(m11),
|
||||
unquote(m12), unquote(m20), unquote(m21), unquote(m22)}
|
||||
|> RotationMatrix.init()
|
||||
|
||||
assert computed == correct
|
||||
end
|
||||
end)
|
||||
end
|
||||
|
|
|
@ -17,68 +17,80 @@ defmodule KinematPointTest do
|
|||
end
|
||||
|
||||
test "cartesian -> cylindrical -> cartesian is correct" do
|
||||
point = Cartesian.init(3, 4, 7)
|
||||
result = point
|
||||
point = Cartesian.init(3, 4, 7)
|
||||
|
||||
result =
|
||||
point
|
||||
|> Point.to_cylindrical()
|
||||
|> Point.to_cartesian()
|
||||
|
||||
pretty_much_equal point.x, result.x
|
||||
pretty_much_equal point.y, result.y
|
||||
pretty_much_equal point.z, result.z
|
||||
pretty_much_equal(point.x, result.x)
|
||||
pretty_much_equal(point.y, result.y)
|
||||
pretty_much_equal(point.z, result.z)
|
||||
end
|
||||
|
||||
test "cartesian -> spherical -> cartesian is correct" do
|
||||
point = Cartesian.init(3, 4, 7)
|
||||
result = point
|
||||
point = Cartesian.init(3, 4, 7)
|
||||
|
||||
result =
|
||||
point
|
||||
|> Point.to_spherical()
|
||||
|> Point.to_cartesian()
|
||||
|
||||
pretty_much_equal point.x, result.x
|
||||
pretty_much_equal point.y, result.y
|
||||
pretty_much_equal point.z, result.z
|
||||
pretty_much_equal(point.x, result.x)
|
||||
pretty_much_equal(point.y, result.y)
|
||||
pretty_much_equal(point.z, result.z)
|
||||
end
|
||||
|
||||
test "cylindrical -> cartesian -> cylindrical" do
|
||||
point = Cylindrical.init(30, ~a(10)d, 9)
|
||||
result = point
|
||||
point = Cylindrical.init(30, ~a(10)d, 9)
|
||||
|
||||
result =
|
||||
point
|
||||
|> Point.to_cartesian()
|
||||
|> Point.to_cylindrical()
|
||||
|
||||
pretty_much_equal point.radial, result.radial
|
||||
pretty_much_equal point.azimuth, result.azimuth
|
||||
pretty_much_equal point.vertical, result.vertical
|
||||
pretty_much_equal(point.radial, result.radial)
|
||||
pretty_much_equal(point.azimuth, result.azimuth)
|
||||
pretty_much_equal(point.vertical, result.vertical)
|
||||
end
|
||||
|
||||
test "cylindrical -> spherical -> cylindrical" do
|
||||
point = Cylindrical.init(30, ~a(10)d, 9)
|
||||
result = point
|
||||
point = Cylindrical.init(30, ~a(10)d, 9)
|
||||
|
||||
result =
|
||||
point
|
||||
|> Point.to_spherical()
|
||||
|> Point.to_cylindrical()
|
||||
|
||||
pretty_much_equal point.radial, result.radial
|
||||
pretty_much_equal point.azimuth, result.azimuth
|
||||
pretty_much_equal point.vertical, result.vertical
|
||||
pretty_much_equal(point.radial, result.radial)
|
||||
pretty_much_equal(point.azimuth, result.azimuth)
|
||||
pretty_much_equal(point.vertical, result.vertical)
|
||||
end
|
||||
|
||||
test "spherical -> cartesian -> spherical" do
|
||||
point = Spherical.init(30, ~a(20)d, ~a(60)d)
|
||||
result = point
|
||||
point = Spherical.init(30, ~a(20)d, ~a(60)d)
|
||||
|
||||
result =
|
||||
point
|
||||
|> Point.to_cartesian()
|
||||
|> Point.to_spherical()
|
||||
|
||||
pretty_much_equal point.radial, result.radial
|
||||
pretty_much_equal point.azimuth, result.azimuth
|
||||
pretty_much_equal point.polar, result.polar
|
||||
pretty_much_equal(point.radial, result.radial)
|
||||
pretty_much_equal(point.azimuth, result.azimuth)
|
||||
pretty_much_equal(point.polar, result.polar)
|
||||
end
|
||||
|
||||
test "spherical -> cylindrical -> spherical" do
|
||||
point = Spherical.init(30, ~a(20)d, ~a(60)d)
|
||||
result = point
|
||||
point = Spherical.init(30, ~a(20)d, ~a(60)d)
|
||||
|
||||
result =
|
||||
point
|
||||
|> Point.to_cylindrical()
|
||||
|> Point.to_spherical()
|
||||
|
||||
pretty_much_equal point.radial, result.radial
|
||||
pretty_much_equal point.azimuth, result.azimuth
|
||||
pretty_much_equal point.polar, result.polar
|
||||
pretty_much_equal(point.radial, result.radial)
|
||||
pretty_much_equal(point.azimuth, result.azimuth)
|
||||
pretty_much_equal(point.polar, result.polar)
|
||||
end
|
||||
end
|
||||
|
|
|
@ -8,23 +8,30 @@ defmodule Kinemat.RotationMatrixToEulerTest do
|
|||
|
||||
# These results were computed using the Mittsu ruby gem.
|
||||
@computed_results %{
|
||||
xyz: {3.141592653589793, 0.0, -1.5707963267948966},
|
||||
yxz: {0.0, 3.141592653589793, 1.5707963267948966},
|
||||
zxy: {0.0, 3.141592653589793, -1.5707963267948966},
|
||||
zyx: {3.141592653589793, 0.0, 1.5707963267948966},
|
||||
yzx: {0.0, 3.141592653589793, 1.5707963267948966},
|
||||
xzy: {3.141592653589793, 0.0, -1.5707963267948966}
|
||||
xyz: {3.141592653589793, 0.0, -1.5707963267948966},
|
||||
yxz: {0.0, 3.141592653589793, 1.5707963267948966},
|
||||
zxy: {0.0, 3.141592653589793, -1.5707963267948966},
|
||||
zyx: {3.141592653589793, 0.0, 1.5707963267948966},
|
||||
yzx: {0.0, 3.141592653589793, 1.5707963267948966},
|
||||
xzy: {3.141592653589793, 0.0, -1.5707963267948966}
|
||||
}
|
||||
|
||||
Enum.each(@computed_results, fn
|
||||
{orientation, {x, y, z}} ->
|
||||
test "converting to #{orientation} Euler" do
|
||||
computed = RotationMatrix.ToEuler.to_euler(@rotation, unquote(orientation))
|
||||
correct = Euler.init(unquote(orientation),
|
||||
Radian.init(unquote(x)),
|
||||
Radian.init(unquote(y)),
|
||||
Radian.init(unquote(z)))
|
||||
assert computed == correct
|
||||
end
|
||||
Enum.each(@computed_results, fn {orientation, {x, y, z}} ->
|
||||
upcased = orientation |> to_string() |> String.upcase()
|
||||
|
||||
test "converting to #{upcased} Euler" do
|
||||
computed =
|
||||
RotationMatrix.ToEuler.to_euler(@rotation, unquote(orientation))
|
||||
|
||||
correct =
|
||||
Euler.init(
|
||||
unquote(orientation),
|
||||
Radian.init(unquote(x)),
|
||||
Radian.init(unquote(y)),
|
||||
Radian.init(unquote(z))
|
||||
)
|
||||
|
||||
assert computed == correct
|
||||
end
|
||||
end)
|
||||
end
|
||||
|
|
|
@ -6,7 +6,12 @@ defmodule Kinemat.RotationMatrixToQuaternionTest do
|
|||
@rotation RotationMatrix.init({0, 1, 0, 1, 0, 0, 0, 0, -1})
|
||||
|
||||
# This value calculated with the Mittsu Ruby gem.
|
||||
@correct Quaternion.init(~a(0.0)r, 0.7071067811865475, 0.7071067811865476, 0.0)
|
||||
@correct Quaternion.init(
|
||||
~a(0.0)r,
|
||||
0.7071067811865475,
|
||||
0.7071067811865476,
|
||||
0.0
|
||||
)
|
||||
|
||||
test "convert rotation matrix to quaternion" do
|
||||
computed = RotationMatrix.ToQuaternion.to_quaternion(@rotation)
|
||||
|
|
Loading…
Reference in a new issue