Solving Inverse-Kinematic Problem for PUMA via Position Based Dynamics
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.
 
 

54 lines
1.4 KiB

#pragma once
#include "Eigen/Dense"
#include <string>
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* (Quaterniond Q, 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(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));
}