joint.h

#pragma once
 
#include <Eigen/Core>
#include <utility>
 
#include "matrix_math.h"
 
 
namespace bmpf {
    class Joint {
    public:
        Joint(Eigen::Matrix4d parentTransform, Eigen::Matrix4d linkTransform,
              bool isFixed, double jointAngle, Eigen::Vector3d axis, bool isVirtual)
                : parentTransform(std::move(parentTransform)), linkTransform(std::move(linkTransform)),
                  isFixed(isFixed), jointAngle(jointAngle), axis(std::move(axis)), isVirtual(isVirtual) {}
 
        Eigen::Matrix4d getTransformMatrix() { return parentTransform * getRotMatrix4x4(axis, jointAngle); }
 
        Eigen::Matrix4d getDiffTransformMatrix() { return parentTransform * getDiffRotMatrix4x4(axis, jointAngle); }
 
        Eigen::Matrix4d getDiff2TransformMatrix() { return parentTransform * getDiff2RotMatrix4x4(axis, jointAngle); }
 
        Eigen::Vector3d getAxis() const { return axis; }
 
        bool isFixed;
        double jointAngle;
        Eigen::Vector3d axis;
        bool isVirtual;
        Eigen::Matrix4d parentTransform;
        Eigen::Matrix4d linkTransform;
    };
 
}