godot-ultraleap-plugin/src/bone.cpp

#include "bone.h"
#include "types.h"

#include <godot_cpp/core/class_db.hpp>

#include <godot_cpp/classes/global_constants.hpp>

#include <godot_cpp/variant/utility_functions.hpp>

using namespace godot;

void UltraleapBone::_bind_methods() {
    ClassDB::bind_method(D_METHOD("get_type"), &UltraleapBone::get_type);
    ClassDB::bind_method(D_METHOD("set_type", "type"), &UltraleapBone::set_type);

    ClassDB::bind_method(D_METHOD("get_prev_joint"), &UltraleapBone::get_prev_joint);
    ClassDB::bind_method(D_METHOD("set_prev_joint", "prev_joint"), &UltraleapBone::set_prev_joint);

    ClassDB::bind_method(D_METHOD("get_next_joint"), &UltraleapBone::get_next_joint);
    ClassDB::bind_method(D_METHOD("set_next_joint", "next_joint"), &UltraleapBone::set_next_joint);

    ClassDB::bind_method(D_METHOD("get_center"), &UltraleapBone::get_center);
    ClassDB::bind_method(D_METHOD("set_center", "center"), &UltraleapBone::set_center);

    ClassDB::bind_method(D_METHOD("get_translation"), &UltraleapBone::get_translation);
    ClassDB::bind_method(D_METHOD("set_translation", "translation"), &UltraleapBone::set_translation);

    ClassDB::bind_method(D_METHOD("get_width"), &UltraleapBone::get_width);
    ClassDB::bind_method(D_METHOD("set_width", "width"), &UltraleapBone::set_width);

    ClassDB::bind_method(D_METHOD("get_length"), &UltraleapBone::get_length);
    ClassDB::bind_method(D_METHOD("set_length", "length"), &UltraleapBone::set_length);

    ClassDB::bind_method(D_METHOD("get_orientation"), &UltraleapBone::get_orientation);
    ClassDB::bind_method(D_METHOD("set_orientation", "orientation"), &UltraleapBone::set_orientation);

    ClassDB::bind_method(D_METHOD("get_rotation"), &UltraleapBone::get_rotation);
    ClassDB::bind_method(D_METHOD("set_rotation", "rotation"), &UltraleapBone::set_rotation);

    ClassDB::add_property(
        "UltraleapBone",
        PropertyInfo(
            Variant::INT,
            "type",
            PROPERTY_HINT_ENUM,
            "Metacarpal,Proximal,Intermediate,Distal,Arm,None"
        ),
        "set_type",
        "get_type"
    );

    ClassDB::add_property(
        "UltraleapBone",
        PropertyInfo(
            Variant::VECTOR3,
            "prev_joint"
        ),
        "set_prev_joint",
        "get_prev_joint"
    );

    ClassDB::add_property(
        "UltraleapBone",
        PropertyInfo(
            Variant::VECTOR3,
            "next_joint"
        ),
        "set_next_joint",
        "get_next_joint"
    );

    ClassDB::add_property(
        "UltraleapBone",
        PropertyInfo(
            Variant::VECTOR3,
            "center"
        ),
        "set_center",
        "get_center"
    );

    ClassDB::add_property(
        "UltraleapBone",
        PropertyInfo(
            Variant::VECTOR3,
            "translation"
        ),
        "set_translation",
        "get_translation"
    );

    ClassDB::add_property(
        "UltraleapBone",
        PropertyInfo(
            Variant::FLOAT,
            "width"
        ),
        "set_width",
        "get_width"
    );

    ClassDB::add_property(
        "UltraleapBone",
        PropertyInfo(
            Variant::FLOAT,
            "length"
        ),
        "set_length",
        "get_length"
    );

    ClassDB::add_property(
        "UltraleapBone",
        PropertyInfo(
            Variant::QUATERNION,
            "orientation"
        ),
        "set_orientation",
        "get_orientation"
    );

    ClassDB::add_property(
        "UltraleapBone",
        PropertyInfo(
            Variant::QUATERNION,
            "rotation"
        ),
        "set_rotation",
        "get_rotation"
    );

    BIND_ENUM_CONSTANT(Metacarpal);
    BIND_ENUM_CONSTANT(Proximal);
    BIND_ENUM_CONSTANT(Intermediate);
    BIND_ENUM_CONSTANT(Distal);
    BIND_ENUM_CONSTANT(Arm);
    BIND_ENUM_CONSTANT(None);
}

void UltraleapBone::fill_bone_data(Ref<UltraleapBone> bone, Ref<UltraleapBone> previous_bone, LEAP_BONE* leap_bone, BoneType type, LEAP_PALM* palm) {
    bone->type = type;
    bone->prev_joint = UltraleapTypes::ultraleap_vector3_to_godot_vector3(&leap_bone->prev_joint);
    bone->next_joint = UltraleapTypes::ultraleap_vector3_to_godot_vector3(&leap_bone->next_joint);
    bone->width = leap_bone->width / 1000;
    bone->orientation = UltraleapTypes::ultraleap_quaternion_to_godot_quaternion(&leap_bone->rotation);

    previous_bone->next_bone = bone;
    bone->previous_bone = previous_bone;

    // If palm is defined, this bone is either a metacarpal or an arm. We still compute relative transform though.
    if (palm != NULL) {
        Transform3D previous_bone_transform = Transform3D(
            Basis(UltraleapTypes::ultraleap_quaternion_to_godot_quaternion(&palm->orientation)),
            UltraleapTypes::ultraleap_vector3_to_godot_vector3(&palm->position)
        );

        Transform3D current_bone_transform = Transform3D(
            Basis(bone->orientation),
            type == UltraleapBone::BoneType::Arm ? bone->next_joint : bone->prev_joint
        );

        Transform3D relative_bone_transform = previous_bone_transform.inverse() * current_bone_transform;
        bone->rotation = relative_bone_transform.basis.get_quaternion();
        bone->translation = relative_bone_transform.origin;
    }
}

void UltraleapBone::compute_bone_data(Ref<UltraleapBone> bone) {
    bone->center = (bone->prev_joint + bone->next_joint) / 2.0f;
    bone->length = (bone->prev_joint - bone->next_joint).length();

    // If previous_bone is defined, we compute the transform from the previous bone to the current bone
    if (bone->previous_bone != NULL) {
        Transform3D previous_bone_transform = Transform3D(
            Basis(bone->previous_bone->orientation),
            bone->previous_bone->prev_joint
        );

        Transform3D current_bone_transform = Transform3D(
            Basis(bone->orientation),
            bone->prev_joint
        );

        Transform3D relative_bone_transform = previous_bone_transform.inverse() * current_bone_transform;
        bone->rotation = relative_bone_transform.basis.get_quaternion();
        bone->translation = relative_bone_transform.origin;
    }
}