116 lines
2.8 KiB
C++
116 lines
2.8 KiB
C++
#ifndef CMVR_ES_COMMON_MATH_PROTO_GEOMETRY_H
|
|
#define CMVR_ES_COMMON_MATH_PROTO_GEOMETRY_H
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
#include "cmvr/common/geometry.pb.h"
|
|
|
|
namespace cmvr::common::math {
|
|
|
|
inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src,
|
|
Eigen::Vector3d defaults)
|
|
{
|
|
if (src.has_x()) {
|
|
defaults.x() = src.x();
|
|
}
|
|
if (src.has_y()) {
|
|
defaults.y() = src.y();
|
|
}
|
|
if (src.has_z()) {
|
|
defaults.z() = src.z();
|
|
}
|
|
return defaults;
|
|
}
|
|
|
|
inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src)
|
|
{
|
|
return toEigenVec3(src, Eigen::Vector3d::Zero());
|
|
}
|
|
|
|
inline Eigen::Matrix<double, 6, 1> toEigenVec6(
|
|
const cmvr::common::Vec6& src,
|
|
Eigen::Matrix<double, 6, 1> defaults)
|
|
{
|
|
if (src.has_x()) {
|
|
defaults[0] = src.x();
|
|
}
|
|
if (src.has_y()) {
|
|
defaults[1] = src.y();
|
|
}
|
|
if (src.has_z()) {
|
|
defaults[2] = src.z();
|
|
}
|
|
if (src.has_rx()) {
|
|
defaults[3] = src.rx();
|
|
}
|
|
if (src.has_ry()) {
|
|
defaults[4] = src.ry();
|
|
}
|
|
if (src.has_rz()) {
|
|
defaults[5] = src.rz();
|
|
}
|
|
return defaults;
|
|
}
|
|
|
|
inline Eigen::Matrix<double, 6, 1> toEigenVec6(const cmvr::common::Vec6& src)
|
|
{
|
|
return toEigenVec6(src, Eigen::Matrix<double, 6, 1>::Zero());
|
|
}
|
|
|
|
inline Eigen::Matrix3d toEigenMat3(const cmvr::common::Mat3& src,
|
|
Eigen::Matrix3d defaults)
|
|
{
|
|
if (src.has_m00()) {
|
|
defaults(0, 0) = src.m00();
|
|
}
|
|
if (src.has_m01()) {
|
|
defaults(0, 1) = src.m01();
|
|
}
|
|
if (src.has_m02()) {
|
|
defaults(0, 2) = src.m02();
|
|
}
|
|
if (src.has_m10()) {
|
|
defaults(1, 0) = src.m10();
|
|
}
|
|
if (src.has_m11()) {
|
|
defaults(1, 1) = src.m11();
|
|
}
|
|
if (src.has_m12()) {
|
|
defaults(1, 2) = src.m12();
|
|
}
|
|
if (src.has_m20()) {
|
|
defaults(2, 0) = src.m20();
|
|
}
|
|
if (src.has_m21()) {
|
|
defaults(2, 1) = src.m21();
|
|
}
|
|
if (src.has_m22()) {
|
|
defaults(2, 2) = src.m22();
|
|
}
|
|
return defaults;
|
|
}
|
|
|
|
inline Eigen::Matrix3d toEigenMat3(const cmvr::common::Mat3& src)
|
|
{
|
|
return toEigenMat3(src, Eigen::Matrix3d::Identity());
|
|
}
|
|
|
|
} // namespace cmvr::common::math
|
|
|
|
|
|
inline bool hasVec3(const cmvr::common::Vec3& value) {
|
|
return value.has_x() && value.has_y() && value.has_z();
|
|
}
|
|
|
|
inline bool hasVec6(const cmvr::common::Vec6& value) {
|
|
return value.has_x() && value.has_y() && value.has_z() &&
|
|
value.has_rx() && value.has_ry() && value.has_rz();
|
|
}
|
|
|
|
inline bool hasMat3(const cmvr::common::Mat3& value) {
|
|
return value.has_m00() && value.has_m01() && value.has_m02() &&
|
|
value.has_m10() && value.has_m11() && value.has_m12() &&
|
|
value.has_m20() && value.has_m21() && value.has_m22();
|
|
}
|
|
#endif // CMVR_ES_COMMON_MATH_PROTO_GEOMETRY_H
|