Body.hpp

// This file is part of the Orbbec Astra SDK [https://orbbec3d.com]
// Copyright (c) 2015-2017 Orbbec 3D
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Be excellent to each other.
#ifndef ASTRA_BODY_HPP
#define ASTRA_BODY_HPP
 
#include <stdexcept>
#include <cstdint>
#include <astra_core/astra_core.hpp>
#include <astra/capi/astra_ctypes.h>
#include <astra/capi/streams/body_capi.h>
#include <astra/capi/streams/stream_types.h>
#include <astra/Array.hpp>
#include <astra/Vector.hpp>
#include <astra/Matrix3x3.hpp>
 
namespace astra {
 
    using std::uint8_t;
    using std::uint16_t;
    using std::int16_t;
    using std::int32_t;
    using std::uint64_t;
 
    class BitmapMask : private ::astra_bitmapmask_t
    {
    public:
        BitmapMask()
        {
            // ::astra_bitmapmask_t::data = nullptr;
            ::astra_bitmapmask_t::width = 0;
            ::astra_bitmapmask_t::height = 0;
        }
 
        int32_t width() const { return ::astra_bitmapmask_t::width; }
        int32_t height() const { return ::astra_bitmapmask_t::height; }
        int32_t bpp() const { return sizeof(uint8_t); }
        int32_t length() const
        {
            return ::astra_bitmapmask_t::width *
                   ::astra_bitmapmask_t::height;
        }
 
        int32_t byte_length() const
        {
            return ::astra_bitmapmask_t::width *
                   ::astra_bitmapmask_t::height *
                   bpp();
        }
 
        const uint8_t* data() const { return ::astra_bitmapmask_t::data; }
 
        friend class FloorInfo;
        friend class BodyFrame;
    };
 
    using BodyMask = BitmapMask;
    using FloorMask = BitmapMask;
 
    enum class BodyTrackingFeatureFlags : ::astra_body_tracking_feature_flags_t
    {
        Segmentation = 0,
        Joints = 1,
        HandPoses = 3
    };
 
    enum class SkeletonProfile : ::astra_skeleton_profile_t {
        Full = 0,
        UpperBody = 1,
        Basic = 2
    };
 
    enum class SkeletonOptimization : ::astra_skeleton_optimization_t {
        Optimization1 = 1,
        MinimizeMemory = 2,
        Optimization2 = 2,
        Optimization3 = 3,
        Optimization4 = 4,
        Balanced = 5,
        Optimization5 = 5,
        Optimization6 = 6,
        Optimization7 = 7,
        Optimization8 = 8,
        Optimization9 = 9,
        BestAccuracy = 9,
    };
 
    enum class BodyOrientation : ::astra_body_orientation_t {
        TOP = 0,
        LEFT = 1,
        RIGHT = 2
    };
 
    enum class JointStatus : ::astra_joint_status_t
    {
        NotTracked = 0,
        LowConfidence = 1,
        Tracked = 2
    };
 
    enum class JointType : ::astra_joint_type_t
    {
        Head            = 0, 
        ShoulderSpine   = 1, 
        LeftShoulder    = 2, 
        LeftElbow       = 3, 
        LeftHand        = 4, 
        RightShoulder   = 5, 
        RightElbow      = 6, 
        RightHand       = 7, 
        MidSpine        = 8, 
        BaseSpine       = 9, 
        LeftHip         = 10, 
        LeftKnee        = 11, 
        LeftFoot        = 12, 
        RightHip        = 13, 
        RightKnee       = 14, 
        RightFoot       = 15, 
        LeftWrist       = 16, 
        RightWrist      = 17, 
        Neck            = 18, 
        Unknown         = 255, 
    };
 
    class Joint : private ::astra_joint_t
    {
    public:
        JointType type() const
        {
            return static_cast<JointType>(::astra_joint_t::type);
        }
 
        JointStatus status() const
        {
            return static_cast<JointStatus>(::astra_joint_t::status);
        }
 
        const Vector2f& depth_position() const
        {
            return *static_cast<const Vector2f*>(&(::astra_joint_t::depthPosition));
        }
 
        const Vector3f& world_position() const
        {
            return *static_cast<const Vector3f*>(&(::astra_joint_t::worldPosition));
        }
 
        const Matrix3x3& orientation() const
        {
            return *static_cast<const Matrix3x3*>(&(::astra_joint_t::orientation));
        }
 
        friend class Body;
    };
 
    enum class HandPose : ::astra_handpose_t
    {
        Unknown = 0,
        Grip = 1,
    };
 
    class HandPoseInfo : private ::astra_handpose_info_t
    {
    private:
        using ::astra_handpose_info_t::leftHand;
        using ::astra_handpose_info_t::rightHand;
    public:
        HandPose left_hand()  const { return static_cast<HandPose>(this->leftHand); }
        HandPose right_hand() const { return static_cast<HandPose>(this->rightHand); }
 
        friend class Body;
    };
 
    enum class BodyStatus : ::astra_body_status_t
    {
        NotTracking = 0,
        TrackingLost = 1,
        TrackingStarted = 2,
        Tracking = 3,
    };
 
    using JointList = Array<const Joint>;
 
    using BodyId = astra_body_id_t;
 
    class Body : private ::astra_body_t
    {
    public:
        BodyId id() const { return ::astra_body_t::id; }
 
        BodyStatus status() const
        {
            return static_cast<BodyStatus>(::astra_body_t::status);
        }
 
        const Vector3f& center_of_mass() const
        {
            return *static_cast<const Vector3f*>(&(::astra_body_t::centerOfMass));
        }
 
        bool joints_enabled() const
        {
            return (::astra_body_t::features & ASTRA_BODY_TRACKING_JOINTS) == ASTRA_BODY_TRACKING_JOINTS;
        }
 
        bool hand_poses_enabled() const
        {
            return (::astra_body_t::features & ASTRA_BODY_TRACKING_HAND_POSES) == ASTRA_BODY_TRACKING_HAND_POSES;
        }
 
        JointList joints() const
        {
            return make_array(
                static_cast<const Joint*>(&(::astra_body_t::joints[0])), ASTRA_MAX_JOINTS);
        }
 
        const HandPoseInfo& hand_poses() const
        {
            return *static_cast<const HandPoseInfo*>(&(::astra_body_t::handPoses));
        }
 
        friend class BodyFrame;
    };
 
    class Plane : private ::astra_plane_t
    {
    public:
        Plane()
        {
            ::astra_plane_t::a = 0.f;
            ::astra_plane_t::b = 0.f;
            ::astra_plane_t::c = 0.f;
            ::astra_plane_t::d = 0.f;
        }
 
        float a() const { return ::astra_plane_t::a; }
        float b() const { return ::astra_plane_t::b; }
        float c() const { return ::astra_plane_t::c; }
        float d() const { return ::astra_plane_t::d; }
 
        const Vector3f& normal() const
        {
            // a, b, c are the normal x, y, z. Use address of 'a'
            // for transmutation of a Vector3f from thin air.
            return *reinterpret_cast<const Vector3f*>(&(::astra_plane_t::a));
        }
 
        bool is_degenerate() const
        {
            return ::astra_plane_t::a == 0.f &&
                   ::astra_plane_t::b == 0.f &&
                   ::astra_plane_t::c == 0.f;
        }
 
        friend class FloorInfo;
    };
 
 
    class FloorInfo : private ::astra_floor_info_t
    {
    private:
        using ::astra_floor_info_t::floorMask;
        using ::astra_floor_info_t::floorPlane;
        using ::astra_floor_info_t::floorDetected;
    public:
        const FloorMask& floor_mask() const
        {
            return *static_cast<const FloorMask*>(&this->floorMask);
        }
        const Plane& floor_plane() const
        {
            return *static_cast<const Plane*>(&this->floorPlane);
        }
        bool floor_detected() const { return this->floorDetected != ASTRA_FALSE; }
 
        friend class BodyFrame;
    };
 
    class BodyFrameInfo : private ::astra_bodyframe_info_t
    {
    public:
        std::int32_t width()  const { return ::astra_bodyframe_info_t::width; }
        std::int32_t height() const { return ::astra_bodyframe_info_t::height; }
        bool is_estimated() const { return ::astra_bodyframe_info_t::isEstimated != 0; }
        friend class BodyFrame;
    };
 
    using BodyList = Array<const Body>;
 
    class BodyFrame
    {
    private:
        ::astra_bodyframe_t handle_;
        astra_frame_index_t frameIndex_;
 
    public:
        BodyFrame(const BodyFrame&) = delete;
        BodyFrame& operator=(const BodyFrame&) = delete;
 
        BodyFrame(BodyFrame&& other)
         : handle_(other.handle_),
           frameIndex_(other.frameIndex_)
        {}
 
        BodyFrame& operator=(BodyFrame&& other)
        {
            handle_ = other.handle_;
            frameIndex_ = other.frameIndex_;
 
            return *this;
        }
 
        template<typename TFrameType>
        static TFrameType acquire(astra_reader_frame_t readerFrame,
                                  astra_stream_subtype_t subtype)
        {
            if (readerFrame != nullptr)
            {
                astra_bodyframe_t bodyFrame;
                astra_frame_get_bodyframe(readerFrame, &bodyFrame);
                return TFrameType(bodyFrame);
            }
 
            return TFrameType(nullptr);
        }
 
        bool is_valid() const { return handle_ != nullptr; }
 
        astra_frame_index_t frame_index() const
        {
            throw_if_invalid_frame();
            return frameIndex_;
        }
 
        const BodyFrameInfo& info() const
        {
            throw_if_invalid_frame();
            return *static_cast<const BodyFrameInfo*>(&handle_->info);
        }
 
        const BodyMask& body_mask() const
        {
            throw_if_invalid_frame();
            return *static_cast<const BodyMask*>(&handle_->bodyMask);
        }
 
        const FloorInfo& floor_info() const
        {
            throw_if_invalid_frame();
            return *static_cast<const FloorInfo*>(&handle_->floorInfo);
        }
 
        BodyList bodies() const
        {
            throw_if_invalid_frame();
            const auto& bodyList = handle_->bodyList;
            return make_array(
                static_cast<const Body*>(&bodyList.bodies[0]), bodyList.count);
        }
 
        void rotate(int angle)
        {
            throw_if_invalid_frame();
            astra_bodyframe_rotate(handle_,angle);
        }
 
    private:
        BodyFrame(astra_bodyframe_t handle)
            : handle_(handle)
        {
            if (handle_)
            {
                astra_bodyframe_get_frameindex(handle, &frameIndex_);
            }
        }
 
        void throw_if_invalid_frame() const
        {
            if (!is_valid())
            {
                throw std::logic_error("Cannot operate on an invalid frame");
            }
        }
    };
 
    
    class BodyStream : public DataStream
    {
    public:
        explicit BodyStream(astra_streamconnection_t connection)
            : DataStream(connection),
              bodyStream_(reinterpret_cast<astra_bodystream_t>(connection))
        { }
 
        BodyTrackingFeatureFlags get_body_features(astra_body_id_t id)
        {
            astra_body_tracking_feature_flags_t features = ASTRA_BODY_TRACKING_SEGMENTATION;
            astra_bodystream_get_body_features(bodyStream_, id, &features);
            return static_cast<BodyTrackingFeatureFlags>(features);
        }
 
        void set_body_features(astra_body_id_t id, BodyTrackingFeatureFlags features)
        {
            astra_bodystream_set_body_features(bodyStream_,
                                               id,
                                               static_cast<astra_body_tracking_feature_flags_t>(features));
        }
 
        BodyTrackingFeatureFlags get_default_body_features()
        {
            astra_body_tracking_feature_flags_t features = ASTRA_BODY_TRACKING_SEGMENTATION;
            astra_bodystream_get_default_body_features(bodyStream_, &features);
            return static_cast<BodyTrackingFeatureFlags>(features);
        }
 
        void set_default_body_features(BodyTrackingFeatureFlags features)
        {
            astra_bodystream_set_default_body_features(bodyStream_,
                                                       static_cast<astra_body_tracking_feature_flags_t>(features));
        }
 
        SkeletonProfile get_skeleton_profile()
        {
            astra_skeleton_profile_t skeletonProfileState = ASTRA_SKELETON_PROFILE_FULL;
            astra_bodystream_get_skeleton_profile(bodyStream_, &skeletonProfileState);
            return static_cast<SkeletonProfile>(skeletonProfileState);
        }
 
        void set_skeleton_profile(SkeletonProfile skeletonProfile)
        {
            astra_bodystream_set_skeleton_profile(bodyStream_,
                                                  static_cast<astra_skeleton_profile_t>(skeletonProfile));
        }
 
        SkeletonOptimization get_skeleton_optimization()
        {
            astra_skeleton_optimization_t skeletonOptimization = ASTRA_SKELETON_OPTIMIZATION_BEST_ACCURACY;
            astra_bodystream_get_skeleton_optimization(bodyStream_, &skeletonOptimization);
            return static_cast<SkeletonOptimization>(skeletonOptimization);
        }
 
        void set_skeleton_optimization(SkeletonOptimization skeletonOptimization)
        {
            astra_bodystream_set_skeleton_optimization(bodyStream_,
                                                       static_cast<astra_skeleton_optimization_t>(skeletonOptimization));
        }
 
        BodyOrientation get_body_orientation()
        {
            astra_body_orientation_t bodyOrientation = ASTRA_BODY_ORIENTATION_TOP;
            astra_bodystream_get_body_orientation(bodyStream_, &bodyOrientation);
            return static_cast<BodyOrientation>(bodyOrientation);
        }
 
        void set_body_orientation(BodyOrientation bodyOrientation)
        {
            astra_bodystream_set_body_orientation(bodyStream_,
                                                  static_cast<astra_body_orientation_t>(bodyOrientation));
        }
 
        static const astra_stream_type_t id = ASTRA_STREAM_BODY;
 
    private:
         astra_bodystream_t bodyStream_{nullptr};
    };
}
 
#endif /* ASTRA_BODY_HPP */