matrixexp3.jl

function MatrixExp3(so3mat::Array)
    omgtheta = so3ToVec(so3mat)
    if NearZero(linalg.norm(omgtheta))
        return linalg.I
    else
        θ = AxisAng3(omgtheta)[2]
        omgmat = so3mat / θ
        return linalg.I + sin(θ) * omgmat + (1 - cos(θ)) * omgmat * omgmat
    end
end