make_cone_look_at Subroutine

private subroutine make_cone_look_at(self, apex, dir, up)

Place the cone's apex at apex (in the parent frame) with its boresight along dir and its +y axis toward up (OpenFrames' makeConeLookAt).

Type Bound

PolyhedralCone

Arguments

Type IntentOptional Attributes Name
class(PolyhedralCone), intent(inout) :: self
real(kind=wp), intent(in) :: apex(3)

parent frame coordinates (scene units)
real(kind=wp), intent(in) :: dir(3)

boresight direction (any length > 0)
real(kind=wp), intent(in) :: up(3)

toward the cone's +y axis (any length)

Calls

proc~~make_cone_look_at~~CallsGraph proc~make_cone_look_at PolyhedralCone%make_cone_look_at proc~cross cross proc~make_cone_look_at->proc~cross proc~quat_from_matrix quat_from_matrix proc~make_cone_look_at->proc~quat_from_matrix proc~set_attitude ReferenceFrame%set_attitude proc~make_cone_look_at->proc~set_attitude proc~set_position~2 ReferenceFrame%set_position proc~make_cone_look_at->proc~set_position~2 proc~next_motion_stamp next_motion_stamp proc~set_attitude->proc~next_motion_stamp proc~quat_normalize quat_normalize proc~set_attitude->proc~quat_normalize proc~set_position~2->proc~next_motion_stamp

Source Code

    subroutine make_cone_look_at(self, apex, dir, up)
        class(PolyhedralCone), intent(inout) :: self
        real(wp), intent(in) :: apex(3)                     !! parent frame coordinates (scene units)
        real(wp), intent(in) :: dir(3)                      !! boresight direction (any length > 0)
        real(wp), intent(in) :: up(3)                       !! toward the cone's +y axis (any length)
        real(wp) :: f(3), s(3), u(3), m(3,3)
        if (norm2(dir) <= 0.0_wp) return
        f = dir / norm2(dir)
        s = cross(f, up)
        if (norm2(s) <= 1.0e-12_wp) s = cross(f, [1.0_wp, 0.0_wp, 0.0_wp])
        if (norm2(s) <= 1.0e-12_wp) s = cross(f, [0.0_wp, 1.0_wp, 0.0_wp])
        s = s / norm2(s)
        u = cross(s, f)
        m(:, 1) = s                                             ! the cone's axes in the parent frame
        m(:, 2) = u
        m(:, 3) = -f
        call self%set_position(apex)
        call self%set_attitude(quat_from_matrix(m))
    end subroutine make_cone_look_at