Class sdurw_math

    • Constructor Detail

      • sdurw_math

        public sdurw_math()
    • Method Detail

      • multiply

        public static VelocityScrew6D multiply​(Jacobian Jq,
                                               Q dq)
        Calculates velocity vector
        Parameters:
        Jq - [in] the jacobian \mathbf{J}_{\mathbf{q}}
        dq - [in] the joint velocity vector \dot{\mathbf{q}}
        Returns:
        the velocity vector \mathbf{\nu}
      • multiply

        public static Q multiply​(Jacobian JqInv,
                                 VelocityScrew6D v)
        Calculates joint velocities

        Parameters:
        JqInv - [in] the inverse jacobian \mathbf{J}_{\mathbf{q}}^{-1}

        v - [in] the velocity vector \mathbf{\nu}

        Returns:
        the joint velocity vector \dot{\mathbf{q}}

      • multiply

        public static Jacobian multiply​(Jacobian j1,
                                        Jacobian j2)
        Multiplies jacobians \mathbf{J} = \mathbf{J}_1 * \mathbf{J}_2

        Parameters:
        j1 - [in] \mathbf{J}_1

        j2 - [in] \mathbf{J}_2

        Returns:
        \mathbf{J}

      • ownedPtr

        public static MetricQPtr ownedPtr​(MetricQ ptr)
        A Ptr that takes ownership over a raw pointer ptr.
      • multiply

        public static PolynomialNDdDouble multiply​(double s,
                                                   PolynomialNDdDouble p)
        Scalar multiplication
        Parameters:
        s - [in] scalar to multiply with.
        p - [in] polynomial to multiply with.
        Returns:
        new polynomial after multiplication.
      • multiply

        public static PolynomialNDfFloat multiply​(float s,
                                                  PolynomialNDfFloat p)
        Scalar multiplication
        Parameters:
        s - [in] scalar to multiply with.
        p - [in] polynomial to multiply with.
        Returns:
        new polynomial after multiplication.
      • divide

        public static Q divide​(double s,
                               Q v)
        Scalar division.
      • dot

        public static double dot​(Q a,
                                 Q b)
        The dot product (inner product) of a and b.
      • concat

        public static Q concat​(Q q1,
                               Q q2)
        concatenates q1 onto q2 such that the returned q has
        the configurations of q1 in [0;q1.size()[ and has q2 in
        [q1.size();q1.size()+q2.size()[
        Parameters:
        q1 - [in] the first Q
        q2 - [in] the second Q
        Returns:
        the concatenation of q1 and q2
      • inverse

        public static Quaternion inverse​(Quaternion q)
        Calculate the inverse Quaternion
        Parameters:
        q - [in] the quaternion being operated on
        Returns:
        the inverse quaternion
      • inverse

        public static Rotation2D inverse​(Rotation2D aRb)
        The inverse \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} of a rotation matrix



        Parameters:
        aRb - [in] the rotation matrix \robabx{a}{b}{\mathbf{R}}

        Returns:
        the matrix inverse \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1}

        \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} = \robabx{a}{b}{\mathbf{R}}^T
      • inverse

        public static Quaternion_f inverse​(Quaternion_f q)
        Calculate the inverse Quaternion
        Parameters:
        q - [in] the quaternion being operated on
        Returns:
        the inverse quaternion
      • inverse

        public static Rotation2Df inverse​(Rotation2Df aRb)
        The inverse \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} of a rotation matrix



        Parameters:
        aRb - [in] the rotation matrix \robabx{a}{b}{\mathbf{R}}

        Returns:
        the matrix inverse \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1}

        \robabx{b}{a}{\mathbf{R}} = \robabx{a}{b}{\mathbf{R}}^{-1} = \robabx{a}{b}{\mathbf{R}}^T
      • multiply

        public static Transform3DVector multiply​(double lhs,
                                                 Transform3DVector rhs)
        Scalar multiplication
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Transform3DVector being multiplied with a scalar
        Returns:
        product of the multiplication
      • add

        public static Transform3DVector add​(double lhs,
                                            Transform3DVector rhs)
        Scalar addition
        Parameters:
        lhs - [in] the scalar to subtraction
        rhs - [in] the Transform3DVector being subtracted from
        Returns:
        the difference
      • subtract

        public static Transform3DVector subtract​(double lhs,
                                                 Transform3DVector rhs)
        Scalar subtraction
        Parameters:
        lhs - [in] the scalar to subtract
        rhs - [in] the Transform3DVector being subtracted from
        Returns:
        the difference
      • divide

        public static Transform3DVector divide​(double lhs,
                                               Transform3DVector rhs)
        Scalar devision
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [in] the Transform3DVector being devided
        Returns:
        the result
      • multiply

        public static Transform3DVector_f multiply​(float lhs,
                                                   Transform3DVector_f rhs)
        Scalar multiplication
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Transform3DVector being multiplied with a scalar
        Returns:
        product of the multiplication
      • add

        public static Transform3DVector_f add​(float lhs,
                                              Transform3DVector_f rhs)
        Scalar addition
        Parameters:
        lhs - [in] the scalar to subtraction
        rhs - [in] the Transform3DVector being subtracted from
        Returns:
        the difference
      • subtract

        public static Transform3DVector_f subtract​(float lhs,
                                                   Transform3DVector_f rhs)
        Scalar subtraction
        Parameters:
        lhs - [in] the scalar to subtract
        rhs - [in] the Transform3DVector being subtracted from
        Returns:
        the difference
      • divide

        public static Transform3DVector_f divide​(float lhs,
                                                 Transform3DVector_f rhs)
        Scalar devision
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [in] the Transform3DVector being devided
        Returns:
        the result
      • multiply

        public static Vector2D multiply​(double s,
                                        Vector2D v)
        Scalar multiplication.
      • multiply

        public static Vector2Df multiply​(float s,
                                         Vector2Df v)
        Scalar multiplication.
      • divide

        public static Vector3D divide​(double lhs,
                                      Vector3D rhs)
        Scalar division.
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [out] the vector beind devided
        Returns:
        result of devision
      • multiply

        public static Vector3D multiply​(double lhs,
                                        Vector3D rhs)
        Scalar multiplication.
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Vector to be multiplied
        Returns:
        the product
      • divide

        public static Vector3Df divide​(float lhs,
                                       Vector3Df rhs)
        Scalar division.
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [out] the vector beind devided
        Returns:
        result of devision
      • multiply

        public static Vector3Df multiply​(float lhs,
                                         Vector3Df rhs)
        Scalar multiplication.
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Vector to be multiplied
        Returns:
        the product
      • divide

        public static Vector6Dd divide​(double lhs,
                                       Vector6Dd rhs)
        Scalar division.
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [out] the vector beind devided
        Returns:
        result of devision
      • multiply

        public static Vector6Dd multiply​(double lhs,
                                         Vector6Dd rhs)
        Scalar multiplication.
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Vector to be multiplied
        Returns:
        the product
      • divide

        public static Vector6Df divide​(float lhs,
                                       Vector6Df rhs)
        Scalar division.
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [out] the vector beind devided
        Returns:
        result of devision
      • multiply

        public static Vector6Df multiply​(float lhs,
                                         Vector6Df rhs)
        Scalar multiplication.
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Vector to be multiplied
        Returns:
        the product
      • divide

        public static Vector5Dd divide​(double lhs,
                                       Vector5Dd rhs)
        Scalar division.
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [out] the vector beind devided
        Returns:
        result of devision
      • multiply

        public static Vector5Dd multiply​(double lhs,
                                         Vector5Dd rhs)
        Scalar multiplication.
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Vector to be multiplied
        Returns:
        the product
      • divide

        public static Vector5Df divide​(float lhs,
                                       Vector5Df rhs)
        Scalar division.
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [out] the vector beind devided
        Returns:
        result of devision
      • multiply

        public static Vector5Df multiply​(float lhs,
                                         Vector5Df rhs)
        Scalar multiplication.
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Vector to be multiplied
        Returns:
        the product
      • divide

        public static Vector4Dd divide​(double lhs,
                                       Vector4Dd rhs)
        Scalar division.
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [out] the vector beind devided
        Returns:
        result of devision
      • multiply

        public static Vector4Dd multiply​(double lhs,
                                         Vector4Dd rhs)
        Scalar multiplication.
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Vector to be multiplied
        Returns:
        the product
      • divide

        public static Vector4Df divide​(float lhs,
                                       Vector4Df rhs)
        Scalar division.
        Parameters:
        lhs - [in] the scalar to devide with
        rhs - [out] the vector beind devided
        Returns:
        result of devision
      • multiply

        public static Vector4Df multiply​(float lhs,
                                         Vector4Df rhs)
        Scalar multiplication.
        Parameters:
        lhs - [in] the scalar to multiply with
        rhs - [in] the Vector to be multiplied
        Returns:
        the product
      • multiply

        public static VelocityScrew6D multiply​(double s,
                                               VelocityScrew6D screw)
        Scales velocity screw and returns scaled version

        Parameters:
        s - [in] scaling value
        screw - [in] Screw to scale
        Returns:
        Scales screw
      • multiply

        public static VelocityScrew6Df multiply​(float s,
                                                VelocityScrew6Df screw)
        Scales velocity screw and returns scaled version

        Parameters:
        s - [in] scaling value
        screw - [in] Screw to scale
        Returns:
        Scales screw
      • cast

        public static EAAf cast​(EAA eaa)
        Casts EAA<T> to EAA<Q>
        Parameters:
        eaa - [in] EAA with type T
        Returns:
        EAA with type Q
      • cast

        public static InertiaMatrixf cast​(InertiaMatrixd rot)
        Casts InertiaMatrix<T> to InertiaMatrix<Q>
        Parameters:
        rot - [in] InertiaMatrix with type T
        Returns:
        InertiaMatrix with type Q
      • cast

        public static Pose6Df cast​(Pose6D pose)
        Casts Pose6D<T> to Pose6D<Q>
        Parameters:
        pose - [in] Pose6D with type T
        Returns:
        Pose6D with type Q
      • cast

        public static Quaternion_f cast​(Quaternion quaternion)
        Casts Quaternion<T> to Quaternion<Q>
        Parameters:
        quaternion - [in] Quarternion with type T
        Returns:
        Quaternion with type Q
      • cast

        public static Rotation2Df cast​(Rotation2D rot)
        Casts Rotation2D<T> to Rotation2D<Q>
        Parameters:
        rot - [in] Rotation2D with type T
        Returns:
        Rotation2D with type R
      • cast

        public static Wrench6Df cast​(Wrench6D vs)
        Casts Wrench6D<T> to Wrench6D<Q>

        Parameters:
        vs - [in] Wrench6D with type T

        Returns:
        Wrench6D with type Q
      • cast

        public static Rotation3Df cast​(Rotation3D rot)
        Casts Rotation3D<T> to Rotation3D<S>



        Parameters:
        rot - [in] Rotation3D with type T
        Returns:
        Rotation3D with type S
      • cast

        public static RPYf cast​(RPY rpy)
        Casts RPY<T> to RPY<Q>

        Parameters:
        rpy - [in] RPY with type T

        Returns:
        RPY with type Q
      • cast

        public static Transform3Df cast​(Transform3D trans)
        Cast Transform3D<T> to Transform3D<Q>
        Parameters:
        trans - [in] Transform3D with type T
        Returns:
        Transform3D with type Q
      • cast

        public static Vector2Df cast​(Vector2D v)
        Casts Vector2D<T> to Vector2D<Q>

        Parameters:
        v - [in] Vector2D with type T

        Returns:
        Vector2D with type Q
      • cast

        public static Vector3Df cast​(Vector3D v)
        Casts Vector3D<T> to Vector3D<Q>
        Parameters:
        v - [in] Vector3D with type T
        Returns:
        Vector3D with type Q

      • cast

        public static VelocityScrew6Df cast​(VelocityScrew6D vs)
        Casts VelocityScrew6D<T> to VelocityScrew6D<Q>

        Parameters:
        vs - [in] VelocityScrew6D with type T

        Returns:
        VelocityScrew6D with type Q
      • cast

        public static EAA cast​(EAAf eaa)
        Casts EAA<T> to EAA<Q>
        Parameters:
        eaa - [in] EAA with type T
        Returns:
        EAA with type Q
      • cast

        public static InertiaMatrixd cast​(InertiaMatrixf rot)
        Casts InertiaMatrix<T> to InertiaMatrix<Q>
        Parameters:
        rot - [in] InertiaMatrix with type T
        Returns:
        InertiaMatrix with type Q
      • cast

        public static Pose6D cast​(Pose6Df pose)
        Casts Pose6D<T> to Pose6D<Q>
        Parameters:
        pose - [in] Pose6D with type T
        Returns:
        Pose6D with type Q
      • cast

        public static Quaternion cast​(Quaternion_f quaternion)
        Casts Quaternion<T> to Quaternion<Q>
        Parameters:
        quaternion - [in] Quarternion with type T
        Returns:
        Quaternion with type Q
      • cast

        public static Rotation2D cast​(Rotation2Df rot)
        Casts Rotation2D<T> to Rotation2D<Q>
        Parameters:
        rot - [in] Rotation2D with type T
        Returns:
        Rotation2D with type R
      • cast

        public static Wrench6D cast​(Wrench6Df vs)
        Casts Wrench6D<T> to Wrench6D<Q>

        Parameters:
        vs - [in] Wrench6D with type T

        Returns:
        Wrench6D with type Q
      • cast

        public static Rotation3D cast​(Rotation3Df rot)
        Casts Rotation3D<T> to Rotation3D<S>



        Parameters:
        rot - [in] Rotation3D with type T
        Returns:
        Rotation3D with type S
      • cast

        public static RPY cast​(RPYf rpy)
        Casts RPY<T> to RPY<Q>

        Parameters:
        rpy - [in] RPY with type T

        Returns:
        RPY with type Q
      • cast

        public static Transform3D cast​(Transform3Df trans)
        Cast Transform3D<T> to Transform3D<Q>
        Parameters:
        trans - [in] Transform3D with type T
        Returns:
        Transform3D with type Q
      • cast

        public static Vector2D cast​(Vector2Df v)
        Casts Vector2D<T> to Vector2D<Q>

        Parameters:
        v - [in] Vector2D with type T

        Returns:
        Vector2D with type Q
      • cast

        public static Vector3D cast​(Vector3Df v)
        Casts Vector3D<T> to Vector3D<Q>
        Parameters:
        v - [in] Vector3D with type T
        Returns:
        Vector3D with type Q

      • cast

        public static VelocityScrew6D cast​(VelocityScrew6Df vs)
        Casts VelocityScrew6D<T> to VelocityScrew6D<Q>

        Parameters:
        vs - [in] VelocityScrew6D with type T

        Returns:
        VelocityScrew6D with type Q
      • transpose

        public static Rotation2D transpose​(Rotation2D aRb)
        Find the transpose of aRb.

        The transpose of a rotation matrix is the same as the inverse.
      • transpose

        public static Rotation2Df transpose​(Rotation2Df aRb)
        Find the transpose of aRb.

        The transpose of a rotation matrix is the same as the inverse.
      • dot

        public static double dot​(Vector a,
                                 Vector b)
        The dot product (inner product) of a and b.

      • dot

        public static double dot​(Vector2D v1,
                                 Vector2D v2)
        Calculates the dot product \mathbf{v1} . \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}

        Returns:
        the dot product \mathbf{v1} . \mathbf{v2}
      • dot

        public static double dot​(Vector3D v1,
                                 Vector3D v2)
        Calculates the dot product \mathbf{v1} . \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}

        Returns:
        the dot product \mathbf{v1} . \mathbf{v2}

      • dot

        public static float dot​(Vectorf a,
                                Vectorf b)
        The dot product (inner product) of a and b.

      • dot

        public static double dot​(Vector2Df v1,
                                 Vector2Df v2)
        Calculates the dot product \mathbf{v1} . \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}

        Returns:
        the dot product \mathbf{v1} . \mathbf{v2}
      • dot

        public static float dot​(Vector3Df v1,
                                Vector3Df v2)
        Calculates the dot product \mathbf{v1} . \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}

        Returns:
        the dot product \mathbf{v1} . \mathbf{v2}

      • cross

        public static double cross​(Vector2D v1,
                                   Vector2D v2)
        Calculates the 2D vector cross product \mathbf{v1} \times \mathbf{v2}

        Parameters:
        v1 - [in] \mathbf{v1}

        v2 - [in] \mathbf{v2}

        Returns:
        the cross product \mathbf{v1} \times \mathbf{v2}

        The 2D vector cross product is defined as:

        \mathbf{v1} \times \mathbf{v2} = v1_x * v2_y - v1_y * v2_x
      • cross

        public static Vector3D cross​(Vector3D v1,
                                     Vector3D v2)
        Calculates the 3D vector cross product \mathbf{v1} \times \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}

        Returns:
        the 3D vector cross product \mathbf{v1} \times \mathbf{v2}

        The 3D vector cross product is defined as:
        \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c} v1_y * v2_z - v1_z * v2_y \\ v1_z * v2_x - v1_x * v2_z \\ v1_x * v2_y - v1_y * v2_x \end{array}\right]

      • cross

        public static void cross​(Vector3D v1,
                                 Vector3D v2,
                                 Vector3D dst)
        Calculates the 3D vector cross product \mathbf{v1} \times \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}
        dst - [out] the 3D vector cross product \mathbf{v1} \times \mathbf{v2}

        The 3D vector cross product is defined as:
        \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c} v1_y * v2_z - v1_z * v2_y \\ v1_z * v2_x - v1_x * v2_z \\ v1_x * v2_y - v1_y * v2_x \end{array}\right]

      • cross

        public static float cross​(Vector2Df v1,
                                  Vector2Df v2)
        Calculates the 2D vector cross product \mathbf{v1} \times \mathbf{v2}

        Parameters:
        v1 - [in] \mathbf{v1}

        v2 - [in] \mathbf{v2}

        Returns:
        the cross product \mathbf{v1} \times \mathbf{v2}

        The 2D vector cross product is defined as:

        \mathbf{v1} \times \mathbf{v2} = v1_x * v2_y - v1_y * v2_x
      • cross

        public static Vector3Df cross​(Vector3Df v1,
                                      Vector3Df v2)
        Calculates the 3D vector cross product \mathbf{v1} \times \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}

        Returns:
        the 3D vector cross product \mathbf{v1} \times \mathbf{v2}

        The 3D vector cross product is defined as:
        \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c} v1_y * v2_z - v1_z * v2_y \\ v1_z * v2_x - v1_x * v2_z \\ v1_x * v2_y - v1_y * v2_x \end{array}\right]

      • cross

        public static void cross​(Vector3Df v1,
                                 Vector3Df v2,
                                 Vector3Df dst)
        Calculates the 3D vector cross product \mathbf{v1} \times \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}
        dst - [out] the 3D vector cross product \mathbf{v1} \times \mathbf{v2}

        The 3D vector cross product is defined as:
        \mathbf{v1} \times \mathbf{v2} = \left[\begin{array}{c} v1_y * v2_z - v1_z * v2_y \\ v1_z * v2_x - v1_x * v2_z \\ v1_x * v2_y - v1_y * v2_x \end{array}\right]

      • angle

        public static double angle​(Vector2D v1,
                                   Vector2D v2)
        calculates the counter clock-wise angle from v1 to
        v2. the value returned will be in the interval [-2Pi,2Pi]
      • angle

        public static double angle​(Vector3D v1,
                                   Vector3D v2,
                                   Vector3D n)
        Calculates the angle from \mathbf{v1} to \mathbf{v2}
        around the axis defined by \mathbf{v1} \times \mathbf{v2} with n
        determining the sign.
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}
        n - [in] \mathbf{n}

        Returns:
        the angle

      • angle

        public static double angle​(Vector3D v1,
                                   Vector3D v2)
        Calculates the angle from \mathbf{v1} to \mathbf{v2}
        around the axis defined by \mathbf{v1} \times \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}

        Returns:
        the angle

      • angle

        public static double angle​(Vector2Df v1,
                                   Vector2Df v2)
        calculates the counter clock-wise angle from v1 to
        v2. the value returned will be in the interval [-2Pi,2Pi]
      • angle

        public static double angle​(Vector3Df v1,
                                   Vector3Df v2,
                                   Vector3Df n)
        Calculates the angle from \mathbf{v1} to \mathbf{v2}
        around the axis defined by \mathbf{v1} \times \mathbf{v2} with n
        determining the sign.
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}
        n - [in] \mathbf{n}

        Returns:
        the angle

      • angle

        public static double angle​(Vector3Df v1,
                                   Vector3Df v2)
        Calculates the angle from \mathbf{v1} to \mathbf{v2}
        around the axis defined by \mathbf{v1} \times \mathbf{v2}
        Parameters:
        v1 - [in] \mathbf{v1}
        v2 - [in] \mathbf{v2}

        Returns:
        the angle

      • normalize

        public static Vector2D normalize​(Vector2D v)
        Returns the normalized vector
        \mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} .

        If \| \mathbf{v} \| = 0 then the zero vector is returned.

        Parameters:
        v - [in] \mathbf{v} which should be normalized

        Returns:
        the normalized vector \mathbf{n}
      • normalize

        public static Vector3D normalize​(Vector3D v)
        Returns the normalized vector \mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} .
        In case \|mathbf{v}\| = 0 the zero vector is returned.
        Parameters:
        v - [in] \mathbf{v} which should be normalized
        Returns:
        the normalized vector \mathbf{n}

      • normalize

        public static Vector2Df normalize​(Vector2Df v)
        Returns the normalized vector
        \mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} .

        If \| \mathbf{v} \| = 0 then the zero vector is returned.

        Parameters:
        v - [in] \mathbf{v} which should be normalized

        Returns:
        the normalized vector \mathbf{n}
      • normalize

        public static Vector3Df normalize​(Vector3Df v)
        Returns the normalized vector \mathbf{n}=\frac{\mathbf{v}}{\|\mathbf{v}\|} .
        In case \|mathbf{v}\| = 0 the zero vector is returned.
        Parameters:
        v - [in] \mathbf{v} which should be normalized
        Returns:
        the normalized vector \mathbf{n}

      • norm1

        public static float norm1​(Wrench6Df wrench)
        Takes the 1-norm of the wrench. All elements both
        force and torque are given the same weight.

        Parameters:
        wrench - [in] the wrench
        Returns:
        the 1-norm
      • norm1

        public static float norm1​(VelocityScrew6Df screw)
        Takes the 1-norm of the velocity screw. All elements both
        angular and linear are given the same weight.

        Parameters:
        screw - [in] the velocity screw
        Returns:
        the 1-norm
      • norm1

        public static double norm1​(Wrench6D wrench)
        Takes the 1-norm of the wrench. All elements both
        force and torque are given the same weight.

        Parameters:
        wrench - [in] the wrench
        Returns:
        the 1-norm
      • norm1

        public static double norm1​(VelocityScrew6D screw)
        Takes the 1-norm of the velocity screw. All elements both
        angular and linear are given the same weight.

        Parameters:
        screw - [in] the velocity screw
        Returns:
        the 1-norm
      • norm2

        public static float norm2​(Wrench6Df wrench)
        Takes the 2-norm of the wrench. All elements both
        force and tporque are given the same weight

        Parameters:
        wrench - [in] the wrench
        Returns:
        the 2-norm
      • norm2

        public static float norm2​(VelocityScrew6Df screw)
        Takes the 2-norm of the velocity screw. All elements both
        angular and linear are given the same weight

        Parameters:
        screw - [in] the velocity screw
        Returns:
        the 2-norm
      • norm2

        public static double norm2​(Wrench6D wrench)
        Takes the 2-norm of the wrench. All elements both
        force and tporque are given the same weight

        Parameters:
        wrench - [in] the wrench
        Returns:
        the 2-norm
      • norm2

        public static double norm2​(VelocityScrew6D screw)
        Takes the 2-norm of the velocity screw. All elements both
        angular and linear are given the same weight

        Parameters:
        screw - [in] the velocity screw
        Returns:
        the 2-norm
      • normInf

        public static float normInf​(Wrench6Df wrench)
        Takes the infinite norm of the wrench. All elements
        both force and torque are given the same weight.

        Parameters:
        wrench - [in] the wrench

        Returns:
        the infinite norm
      • normInf

        public static float normInf​(VelocityScrew6Df screw)
        Takes the infinite norm of the velocity screw. All elements
        both angular and linear are given the same weight.

        Parameters:
        screw - [in] the velocity screw

        Returns:
        the infinite norm
      • normInf

        public static double normInf​(Wrench6D wrench)
        Takes the infinite norm of the wrench. All elements
        both force and torque are given the same weight.

        Parameters:
        wrench - [in] the wrench

        Returns:
        the infinite norm
      • normInf

        public static double normInf​(VelocityScrew6D screw)
        Takes the infinite norm of the velocity screw. All elements
        both angular and linear are given the same weight.

        Parameters:
        screw - [in] the velocity screw

        Returns:
        the infinite norm
      • castToFloat

        public static EAAf castToFloat​(EAA eaa)
        Casts EAA<T> to EAA<Q>
        Parameters:
        eaa - [in] EAA with type T
        Returns:
        EAA with type Q
      • castToFloat

        public static InertiaMatrixf castToFloat​(InertiaMatrixd rot)
        Casts InertiaMatrix<T> to InertiaMatrix<Q>
        Parameters:
        rot - [in] InertiaMatrix with type T
        Returns:
        InertiaMatrix with type Q
      • castToFloat

        public static Pose6Df castToFloat​(Pose6D pose)
        Casts Pose6D<T> to Pose6D<Q>
        Parameters:
        pose - [in] Pose6D with type T
        Returns:
        Pose6D with type Q
      • castToFloat

        public static Quaternion_f castToFloat​(Quaternion quaternion)
        Casts Quaternion<T> to Quaternion<Q>
        Parameters:
        quaternion - [in] Quarternion with type T
        Returns:
        Quaternion with type Q
      • castToFloat

        public static Rotation2Df castToFloat​(Rotation2D rot)
        Casts Rotation2D<T> to Rotation2D<Q>
        Parameters:
        rot - [in] Rotation2D with type T
        Returns:
        Rotation2D with type R
      • castToFloat

        public static Wrench6Df castToFloat​(Wrench6D vs)
        Casts Wrench6D<T> to Wrench6D<Q>

        Parameters:
        vs - [in] Wrench6D with type T

        Returns:
        Wrench6D with type Q
      • castToFloat

        public static Rotation3Df castToFloat​(Rotation3D rot)
        Casts Rotation3D<T> to Rotation3D<S>



        Parameters:
        rot - [in] Rotation3D with type T
        Returns:
        Rotation3D with type S
      • castToFloat

        public static RPYf castToFloat​(RPY rpy)
        Casts RPY<T> to RPY<Q>

        Parameters:
        rpy - [in] RPY with type T

        Returns:
        RPY with type Q
      • castToFloat

        public static Transform3Df castToFloat​(Transform3D trans)
        Cast Transform3D<T> to Transform3D<Q>
        Parameters:
        trans - [in] Transform3D with type T
        Returns:
        Transform3D with type Q
      • castToFloat

        public static Vector2Df castToFloat​(Vector2D v)
        Casts Vector2D<T> to Vector2D<Q>

        Parameters:
        v - [in] Vector2D with type T

        Returns:
        Vector2D with type Q
      • castToFloat

        public static Vector3Df castToFloat​(Vector3D v)
        Casts Vector3D<T> to Vector3D<Q>
        Parameters:
        v - [in] Vector3D with type T
        Returns:
        Vector3D with type Q

      • castToFloat

        public static VelocityScrew6Df castToFloat​(VelocityScrew6D vs)
        Casts VelocityScrew6D<T> to VelocityScrew6D<Q>

        Parameters:
        vs - [in] VelocityScrew6D with type T

        Returns:
        VelocityScrew6D with type Q
      • castToDouble

        public static EAA castToDouble​(EAAf eaa)
        Casts EAA<T> to EAA<Q>
        Parameters:
        eaa - [in] EAA with type T
        Returns:
        EAA with type Q
      • castToDouble

        public static InertiaMatrixd castToDouble​(InertiaMatrixf rot)
        Casts InertiaMatrix<T> to InertiaMatrix<Q>
        Parameters:
        rot - [in] InertiaMatrix with type T
        Returns:
        InertiaMatrix with type Q
      • castToDouble

        public static Pose6D castToDouble​(Pose6Df pose)
        Casts Pose6D<T> to Pose6D<Q>
        Parameters:
        pose - [in] Pose6D with type T
        Returns:
        Pose6D with type Q
      • castToDouble

        public static Quaternion castToDouble​(Quaternion_f quaternion)
        Casts Quaternion<T> to Quaternion<Q>
        Parameters:
        quaternion - [in] Quarternion with type T
        Returns:
        Quaternion with type Q
      • castToDouble

        public static Rotation2D castToDouble​(Rotation2Df rot)
        Casts Rotation2D<T> to Rotation2D<Q>
        Parameters:
        rot - [in] Rotation2D with type T
        Returns:
        Rotation2D with type R
      • castToDouble

        public static Wrench6D castToDouble​(Wrench6Df vs)
        Casts Wrench6D<T> to Wrench6D<Q>

        Parameters:
        vs - [in] Wrench6D with type T

        Returns:
        Wrench6D with type Q
      • castToDouble

        public static Rotation3D castToDouble​(Rotation3Df rot)
        Casts Rotation3D<T> to Rotation3D<S>



        Parameters:
        rot - [in] Rotation3D with type T
        Returns:
        Rotation3D with type S
      • castToDouble

        public static RPY castToDouble​(RPYf rpy)
        Casts RPY<T> to RPY<Q>

        Parameters:
        rpy - [in] RPY with type T

        Returns:
        RPY with type Q
      • castToDouble

        public static Transform3D castToDouble​(Transform3Df trans)
        Cast Transform3D<T> to Transform3D<Q>
        Parameters:
        trans - [in] Transform3D with type T
        Returns:
        Transform3D with type Q
      • castToDouble

        public static Vector2D castToDouble​(Vector2Df v)
        Casts Vector2D<T> to Vector2D<Q>

        Parameters:
        v - [in] Vector2D with type T

        Returns:
        Vector2D with type Q
      • castToDouble

        public static Vector3D castToDouble​(Vector3Df v)
        Casts Vector3D<T> to Vector3D<Q>
        Parameters:
        v - [in] Vector3D with type T
        Returns:
        Vector3D with type Q

      • castToDouble

        public static VelocityScrew6D castToDouble​(VelocityScrew6Df vs)
        Casts VelocityScrew6D<T> to VelocityScrew6D<Q>

        Parameters:
        vs - [in] VelocityScrew6D with type T

        Returns:
        VelocityScrew6D with type Q