// Fill out your copyright notice in the Description page of Project Settings.


#include "Link.h"

#include <iostream>

#include "Eigen/Dense"

Eigen::Matrix3d ULink::GetMomentOfInertia() const {
	const FVector FI = this->GetInertiaTensor();

	// Inertial tensor of rotation
	const Eigen::Vector3d I(FI.X, FI.Y, FI.Z);

	// Moment of inertia
	const Eigen::Matrix3d I0;// = R.transpose() * I * R;

	return I0;
}

Eigen::Matrix3d ULink::GetRotationMatrix() const{
	const FQuat fq = this->GetRelativeRotation().Quaternion();
	const Eigen::Vector3d q(fq.X, fq.Y, fq.Z);
	const double w = fq.W;
	Eigen::Matrix3d q_hat;
	q_hat << 0, -q.z(), q.y(),
			 q.z(), 0, -q.x(),
			 -q.y(), q.x(), 0;
	const Eigen::Matrix3d R = (w * w - q.dot(q)) * Eigen::Matrix3d::Identity() + 2 * q * q.transpose() + 2 * w * q_hat;
	return R;
}

void ULink::BeginPlay(){
	const FVector t = this->GetInertiaTensor();
	inertia_tensor = Eigen::Vector3d(t.X, t.Y, t.Z);
	
	const double M = this->CalculateMass();
	Eigen::Vector3d v;
	v << 3, 5, 7;
	
	UE_LOG(LogTemp, Log, TEXT("%f"), M);
}