PBD-Robotics/Source/PBDRobotics/util.h

#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* (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 Quaterniond operator-= (const Quaterniond Q1, const Quaterniond Q2){
	return Q1 - Q2;
}