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.
43 lines
1.0 KiB
43 lines
1.0 KiB
// 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);
|
|
}
|
|
|