You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
57 lines
1.5 KiB
57 lines
1.5 KiB
#pragma once
|
|
|
|
#include "Eigen/Dense"
|
|
|
|
inline Vector3d ToEigen(const FVector V){
|
|
return Vector3d(V.X, V.Y, V.Z);
|
|
}
|
|
|
|
inline Quaterniond ToEigen(const FQuat Q){
|
|
return Quaterniond(Q.W, Q.X, Q.Y, Q.Z);
|
|
}
|
|
|
|
inline FVector FromEigen(const Vector3d& V){
|
|
return FVector(V.x(), V.y(), V.z());
|
|
}
|
|
|
|
inline FQuat FromEigen(const Quaterniond& Q){
|
|
return FQuat(Q.x(), Q.y(), Q.z(), Q.w());
|
|
}
|
|
|
|
inline Quaterniond operator* (const Quaterniond& Q, const double D){
|
|
return Quaterniond(Q.w() * D, Q.x() * D, Q.y() * D, Q.z() * D);
|
|
}
|
|
|
|
inline Quaterniond operator+ (const Quaterniond& Q1, const Quaterniond& Q2){
|
|
return Quaterniond(Q1.w() + Q2.w(), Q1.x() + Q2.x(), Q1.y() + Q2.y(), Q1.z() + Q2.z());
|
|
}
|
|
|
|
inline Quaterniond operator- (const Quaterniond& Q1, const Quaterniond& Q2){
|
|
return Quaterniond(Q1.w() - Q2.w(), Q1.x() - Q2.x(), Q1.y() - Q2.y(), Q1.z() - Q2.z());
|
|
}
|
|
|
|
inline void Log(const MatrixXd& M){
|
|
assert(M.rows() == 3);
|
|
if (M.cols() == 3){
|
|
UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(0, 0), M(0, 1), M(0, 2));
|
|
UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(1, 0), M(1, 1), M(1, 2));
|
|
UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(2, 0), M(2, 1), M(2, 2));
|
|
}
|
|
if (M.cols() == 1){
|
|
UE_LOG(LogTemp, Log, TEXT("%f %f %f"), M(0), M(1), M(2));
|
|
}
|
|
}
|
|
|
|
inline void Log(const double V){
|
|
UE_LOG(LogTemp, Log, TEXT("%f"), V);
|
|
}
|
|
|
|
inline void Log(const char* S){
|
|
UE_LOG(LogTemp, Log, TEXT("%s"), *FString(S));
|
|
}
|
|
|
|
inline void Log(const Quaterniond& Q){
|
|
UE_LOG(LogTemp, Log, TEXT("%f %f %f | %f"), Q.x(), Q.y(), Q.z(), Q.w());
|
|
}
|
|
|
|
|
|
|