Functions

Modia3D.ZeroVector3D(::Type{F}) where F = SVector{3,F}(0, 0, 0)

Constant of a SVector{3,Float64} where all elements are zero

vec = Modia3D.axisValue(axis, value::F) where F
vec = Modia3D.axisValue(axis, positive, value::F) where F

Return vec::SVector{3,F} where all elements are zero with exception of vec[axis] = value or vec[axis] = positive ? value : -value.

R = Modia3D.MullRotation(::Type{F}) where F

Return rotation matrix R::SMatrix{3,3,F,9} that defines no rotation from frame 1 to frame 2.

Modia3D.assertRotationMatrix(R::AbstractMatrix)

Assert that matrix R has the properties of a rotation matrix (is 3x3 and R'*R - eye(3) = zeros(3,3))

R = Modia3D.rot1(angle)

Return rotation matrix R that rotates with angle angle along the x-axis of frame 1. angle can be provided in radian or in degree, e.g. using Unitful; Modia3D.rot1(90u"°").

Modia3D.rot1(angle,v::AbstractVector)

Return in principal Modia3D.rot1(angle)*v, but compute this efficiently by taking the zeros in rot1(..) into account. angle can be provided in radian or in degree, e.g. using Unitful; Modia3D.rot1(90u"°", v).

R = Modia3D.rot2(angle)

Return rotation matrix R that rotates with angle angle along the y-axis of frame 1. angle can be provided in radian or in degree, e.g. using Unitful; Modia3D.rot2(90u"°").

Modia3D.rot2(angle,v::AbstractVector)

Return in principal Modia3D.rot2(angle)*v, but compute this efficiently by taking the zeros in rot2(..) into account. angle can be provided in radian or in degree, e.g. using Unitful; Modia3D.rot2(90u"°", v).

R = Modia3D.rot3(angle)

Return rotation matrix R that rotates with angle angle along the z-axis of frame 1. angle can be provided in radian or in degree, e.g. using Unitful; Modia3D.rot3(90u"°").

Modia3D.rot3(angle,v::AbstractVector)

Return in principal Modia3D.rot3(angle)*v, but compute this efficiently by taking the zeros in rot3(..) into account. angle can be provided in radian or in degree, e.g. using Unitful; Modia3D.rot3(90u"°", v).

R = Modia3D.rot123(angle1, angle2, angle3)
R = Modia3D.rot123(angles)

Return rotation matrix R by rotating with angle1 along the x-axis of frame 1, then with angle2 along the y-axis of this frame and then with angle3 along the z-axis of this frame. The angles can be optionally provided in form of a vector angles = [angle1, angle2, angle3].

R = Modia3D.rotAxis(axis, angle)
R = Modia3D.rotAxis(axis, positive, angle)

Return rotation matrix R that rotates with angle angle along axis axis (= 1, 2 or 3), or with angle if positive=true and otherwise with -angle. angle can be provided in radian or in degree, e.g. using Unitful; Modia3D.rotAxis(2, 90u"°").

R = Modia3D.rot_e(e, angle)

Return rotation matrix that rotates frame1 around angle along unit axis e arriving at frame2. This function assumes that norm(e) == 1.

R = Modia3D.rot_nxy(nx, ny)

It is assumed that the two input vectors nx and ny are resolved in frame 1 and are directed along the x and y axis of frame 2. The function returns the rotation matrix R to rotate from frame 1 to frame 2.

The function is robust in the sense that it returns always a rotation matrix R, even if ny is not orthogonal to nx or if one or both vectors have zero length. This is performed in the following way: If nx and ny are not orthogonal to each other, first a unit vector ey is determined that is orthogonal to nx and is lying in the plane spanned by nx and ny. If nx and ny are parallel or nearly parallel to each other or ny is a vector with zero or nearly zero length, a vector ey is selected arbitrarily such that ex and ey are orthogonal to each other. If both nx and ny are vectors with zero or nearly zero length, an arbitrary rotation matrix is returned.

Example

using Unitful
import Modia3D

R1 = Modia3D.rot1(90u"°")
R2 = Modia3D.rot_nxy([1  , 0, 0], [0  , 0, 1  ])
R3 = Modia3D.rot_nxy([0.9, 0, 0], [1.1, 0, 1.1])
isapprox(R1,R2)   # returns true
isapprox(R1,R3)   # returns true

R = Modia3D.rot_nxz(nx, nz)

It is assumed that the two input vectors nx and nz are resolved in frame 1 and are directed along the x and y axis of frame 2. The function returns the rotation matrix R to rotate from frame 1 to frame 2.

The function is robust in the sense that it returns always a rotation matrix R, even if nx is not orthogonal to nz or if one or both vectors have zero length. This is performed in the following way: If nx and nz are not orthogonal to each other, first a unit vector ex is determined that is orthogonal to nz and is lying in the plane spanned by nx and nz. If nx and nz are parallel or nearly parallel to each other or nx is a vector with zero or nearly zero length, a vector ex is selected arbitrarily such that ex and ez are orthogonal to each other. If both nx and nz are vectors with zero or nearly zero length, an arbitrary rotation matrix is returned.

Example

using Unitful
import Modia3D

R1 = Modia3D.rot3(90u"°")
R2 = Modia3D.rot_nxz([0, 1  , 0  ], [0, 0, 1  ])
R3 = Modia3D.rot_nxz([0, 1.1, 1.1], [0, 0, 0.9])
isapprox(R1,R2)   # returns true
isapprox(R1,R3)   # returns true

R = Modia3D.from_q(q)

Return rotation matrix R::SMatrix{3,3,F,9} from quaternions q::SVector{4,F}.

const Modia3D.NullQuaternion(F) = SVector{4,F}(0.0, 0.0, 0.0, 1.0)

Constant quaternion vector of a null rotation (= no rotation from frame 1 to frame 2)

Modia3D.assertQuaternion(q::AbstractVector)

Assert that vector q has the properties of a quaternion vector (has 4 elements, norm(q) = 1)

q = Modia3D.qrot1(angle; q_guess = NullQuaternion(F))

Return quaternion q that rotates with angle angle along the x-axis of frame 1.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = Modia3D.qrot2(angle; q_guess = NullQuaternion(F))

Return quaternion q that rotates with angle angle along the y-axis of frame 1.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = Modia3D.qrot3(angle; q_guess = NullQuaternion(F))

Return quaternion q that rotates with angle angle along the z-axis of frame 1.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = Modia3D.qrot123(angle1, angle2, angle3)

Return quaternion q by rotating with angle1 along the x-axis of frame 1, then with angle2 along the y-axis of this frame and then with angle3 along the z-axis of this frame.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = Modia3D.qrot_e(e, angle; q_guess = NullQuaternion(F))

Return quaternion q that rotates with angle angle along unit axis e. This function assumes that norm(e) == 1.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

q = Modia3D.qrot_nxy(nx, ny)

It is assumed that the two input vectors nx and ny are resolved in frame 1 and are directed along the x and y axis of frame 2. The function returns the quaternion q to rotate from frame 1 to frame 2.

The function is robust in the sense that it returns always a quaternion q, even if ny is not orthogonal to nx or if one or both vectors have zero length. This is performed in the following way: If nx and ny are not orthogonal to each other, first a unit vector ey is determined that is orthogonal to nx and is lying in the plane spanned by nx and ny. If nx and ny are parallel or nearly parallel to each other or ny is a vector with zero or nearly zero length, a vector ey is selected arbitrarily such that ex and ey are orthogonal to each other. If both nx and ny are vectors with zero or nearly zero length, an arbitrary quaternion q is returned.

Example

using Unitful
import Modia3D

q1 = Modia3D.qrot1(90u"°")
q2 = Modia3D.qrot_nxy([1  , 0, 0], [0  , 0, 1  ])
q3 = Modia3D.qrot_nxy([0.9, 0, 0], [1.1, 0, 1.1])
isapprox(q1,q2)   # returns true
isapprox(q1,q3)   # returns true

q = Modia3D.qrot_nxz(nx, nz)

It is assumed that the two input vectors nx and nz are resolved in frame 1 and are directed along the x and z axis of frame 2. The function returns the quaternion q to rotate from frame 1 to frame 2.

The function is robust in the sense that it returns always a quaternion q, even if nx is not orthogonal to nz or if one or both vectors have zero length. This is performed in the following way: If nx and nz are not orthogonal to each other, first a unit vector ex is determined that is orthogonal to nz and is lying in the plane spanned by nx and nz. If nx and nz are parallel or nearly parallel to each other or nx is a vector with zero or nearly zero length, a vector ex is selected arbitrarily such that ex and ez are orthogonal to each other. If both nx and nz are vectors with zero or nearly zero length, an arbitrary quaternion q is returned.

Example

using Unitful
import Modia3D

q1 = Modia3D.qrot3(90u"°")
q2 = Modia3D.qrot_nxz([0, 1  , 0  ], [0, 0, 1  ])
q3 = Modia3D.qrot_nxz([0, 1.1, 1.1], [0, 0, 0.9])
isapprox(q1,q2)   # returns true
isapprox(q1,q3)   # returns true

q = Modia3D.from_R(R; q_guess = NullQuaternion(F))

Return quaternion q::SVector{4,F} from rotation matrix R::SMatrix{3,3,F,9}.

From the two possible solutions q the one is returned that is closer to q_guess (note, q and -q define the same rotation).

v1 = Modia3D.resolve1(R, v2)
v1 = Modia3D.resolve1(q, v2)
v1 = Modia3D.resolve1(rotation, v2; rotationXYZ=true)

Transform vector v2::AbstractVector (vector resolved in frame 2) to vector v1::SVector{3,F} (vector resolved in frame 1) given

• Rotation matrix R::SMatrix{3,3,F,9} (rotate frame 1 into frame 2) or
• Quaternion vector q::SVector{4,F} (rotate frame 1 into frame 2) or
• Angles vector rotation::AbstractVector (rotationXYZ=true: rotate around X with rotation[1], around Y with rotation[2], around Z with rotation[3]; rotationXYZ=false: rotate around X with rotation[1], around Z with rotation[2], around Y with rotation[3]).

v2 = Modia3D.resolve2(R, v1)
v2 = Modia3D.resolve2(q, v1)
v2 = Modia3D.resolve2(rotation, v1; rotationXYZ=true)

Transform vector v1::AbstractVector (vector resolved in frame 1) to vector v2::SVector{3,F} (vector resolved in frame 2) given

• Rotation matrix R::SMatrix{3,3,F,9} (rotate frame 1 into frame 2) or
• Quaternion vector q::SVector{4,F} (rotate frame 1 into frame 2) or
• Angles vector rotation::AbstractVector (rotationXYZ=true: rotate around X with rotation[1], around Y with rotation[2], around Z with rotation[3]; rotationXYZ=false: rotate around X with rotation[1], around Z with rotation[2], around Y with rotation[3]).

 R2 = Modia3D.absoluteRotation(R1, R_rel)
 q2 = Modia3D.absoluteRotation(q1, q_rel)

Return rotation matrix R2 or quaternion q2 defining the rotation from frame 0 to frame 2 from rotation matrix R1 or quaternion q1that define the rotation from frame 0 to frame 1 and the relative rotation matrix R_rel or the relative quaternion q_rel that define the rotation from frame 1 to frame 2.

 R_rel = Modia3D.relativeRotation(R1, R2)
 q_rel = Modia3D.relativeRotation(q1, q2)

Return relative rotation matrix R_rel or relative quaternion q_rel defining the rotation from frame 1 to frame 2 from absolute rotation matrix R1 or absolute quaternion q1 that define the rotation from frame 0 to frame 1 and the absolute rotation matrix R2 or the absolute quaternion q2 that define the rotation from frame 0 to frame 2.

 R_inv = Modia3D.inverseRotation(R)
 q_inv = Modia3D.inverseRotation(q)

Return inverse rotation matrix R_inv or inverse quaternion q_inv defining the rotation from frame 1 to frame 0 from rotation matrix R or quaternion q that define the rotation from frame 0 to frame 1.

angle = planarRotationAngle(e, v1, v2; angle_guess = 0.0)

Return angle of a planar rotation, given the normalized axis of rotation to rotate frame 1 around e into frame 2 (norm(e) == 1 required), and the representations of a vector in frame 1 (v1) and frame 2 (v2). Hereby, it is required that v1 is not parallel to e. The returned angle is in the range -pi <= angle - angle_guess <= pi (from the infinite many solutions, the one is returned that is closest to angle_guess).

Example

import Modia3D
using Unitful

angle1 = 45u"°"
e      = normalize([1.0, 1.0, 1.0])
R      = Modia3D.rot_e(e, angle1)

v1 = [1.0, 2.0, 3.0]
v2 = Modia3D.resolve2(R, v1)

angle2 = planarRotationAngle(e, v1, v2)
isapprox(angle1, angle2)

angle = planarRotationAngle(frame1, frame2)

Under the assumption that the z-axes of frame1 and frame2 coincide, return the angle between the x-axis of frame1 and the position vector from frame1 to frame2.

e = eAxis(::Type{F}, axis::Int)

Return unit vector e::SVector{3,F} in direction of axis axis (axis = 1,2,3 or -1,-2-,3).

Example

import Modia3D

e1 = ModiMath.eAxis(1)    # e1 = SVector{3,F}(1.0,  0.0, 0.0)
e2 = ModiMath.eAxis(-2)   # d2 = SVector{3,F}(0.0, -1.0, 0.0)

M = Modia3D.skew(e::SVector{3,F}) where F
M = Modia3D.skew(e::Vector{F})    where F

Return the skew symmetric matrix M of vector e (length(e) = 3) with

M = @SMatrix [  F(0.0)  -e[3]    e[2];
                e[3]    F(0.0)  -e[1];
               -e[2]     e[1]   F(0.0) ]

path = Modia3D.Path(r::Vector{SVector{3,Float64}},
                    q::Vector{SVector{4,Float64}} = NullQuaternion(Float64);
                    v = ones(length(r)))

Return an instance of a new Path object. The Path object consists of n frames defined by the position vectors of their origins (r[i] for frame i) and optionally of their absolute rotation quaternions (q[i] for frame i) describing the rotation from the world frame to the respective frame.

A path parameter t is defined in the following way on these frames:

• t[1] = 0.0.
• t[i] = t[i-1] + pathLength_i/(v[i]+v[i-1])/2 if the origins of frames i-1 and i do not coincide.
• t[i] = t[i-1] + pathAngle_i/(v[i]+v[i-1])/2 if the origins of frames i-1 and i do coincide.

Hereby pathLength_i is the distance between the origins of frames i-1 and i in [m] and pathAngle_i is the planar rotation angle between frames i-1 and i in [rad].

If v[i] is the desired velocity or angular velocity at frame i, then path parameter t is approximately the time to move along the path. The time instant t_end of the last frame can be inquired with Modia3D.t_pathEnd(path). For example, if a simulation shall be performed in such a way that the simulation should start with the first frame and end at stopTime at the last frame, then the path parameter should be selected as t = time*t_end/stopTime.

Given the actual path parameter, typically 0 <= t <= t_end (if t is outside of this interval, the frame at t is determined by extrapolation through the first two or the last two frames), the corresponding frame is determined by linear interpolation in the following way:

(rt, qt) = interpolate(  path,t)
 rt      = interpolate_r(path,t)

where rt is the position vector to the origin of the frame at path parameter t and qt is the absolute quaternion of the frame at path parameter t.

Example

import Modia3D
using Unitful

r = [ SVector{3,F}(1,0,0),
      SVector{3,F}(0,1,0),
      SVector{3,F}(0,0,1) ]
q = [ Modia3D.NullQuaternion(F),
      Modia3D.qrot1(45u"°"),
      Modia3D.qrot2(60u"°")]

path     = Modia3D.Path(r,q)
t_end    = Modia3D.t_pathEnd(path)
dt       = 0.1
stopTime = 2.0
time     = 0.0

while time <= stopTime
   (rt, qt) = Modia3D.interpolate(path, time*t_end/stopTime)
   time += dt
end

t_end = Modia3D.t_pathEnd(path::[`Modia3D.Path`](@ref))

Return the final path parameter tof the last frame in path (path parameter of first frame = 0.0).

(rt, qt) = Modia3D.interpolate(path, t)

Return position rtand Quaternion qt of path::Modia3D.Path at path parameter t::Number.

rt = Modia3D.interpolate_r(path, t)

Return position r of path::Modia3D.Path at path parameter t::Number.