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.
96 lines
2.7 KiB
96 lines
2.7 KiB
// Fill out your copyright notice in the Description page of Project Settings.
|
|
|
|
|
|
#include "Link.h"
|
|
|
|
#include <iostream>
|
|
|
|
#include "util.h"
|
|
#include "Eigen/Dense"
|
|
#include "Engine/StaticMeshSocket.h"
|
|
|
|
using namespace Eigen;
|
|
|
|
Matrix3d ULink::GetInertia() const {
|
|
return Orientation.toRotationMatrix() * Inertia_Tensor_Local;
|
|
}
|
|
|
|
double ULink::GetPositionalInverseMass(const Vector3d R, const Vector3d N) const{
|
|
const double M = Mass;
|
|
const Matrix3d I = GetInertia();
|
|
return 1 / M + R.cross(N).transpose() * I.inverse() * R.cross(N);
|
|
}
|
|
double ULink::GetRotationalInverseMass(const Vector3d N) const{
|
|
const Matrix3d I = GetInertia();
|
|
return N.transpose() * I.inverse() * N;
|
|
}
|
|
|
|
void ULink::Setup(){
|
|
SetupJoints();
|
|
SetupProperties();
|
|
}
|
|
|
|
void ULink::SetupProperties(){
|
|
Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
|
|
Position = ToEigen(GetComponentLocation());
|
|
Velocity = Vector3d::Zero();
|
|
Mass = CalculateMass();
|
|
Orientation = ToEigen(GetComponentQuat());
|
|
AngularVelocity = Vector3d::Zero();
|
|
|
|
if (IsBase){
|
|
Mass = 1e30;
|
|
Inertia_Tensor_Local = Vector3d::Ones().asDiagonal() * 1e30;
|
|
}
|
|
|
|
const Matrix3d I = GetInertia().inverse();
|
|
UE_LOG(LogTemp, Log, TEXT("%s"), *GetName());
|
|
auto log = [this, I](int col){
|
|
UE_LOG(LogTemp, Log, TEXT("%f %f %f"), I(col, 0), I(col, 1), I(col, 2));
|
|
};
|
|
log(0);log(1);log(2);
|
|
|
|
}
|
|
|
|
void ULink::SetupJoints() const {
|
|
if (const UStaticMeshSocket* Socket = GetSocketByName("JointPrev")){
|
|
if (PrevJoint){
|
|
PrevJoint->SecondLocalPosition = ToEigen(Socket->RelativeLocation);
|
|
PrevJoint->SecondRotateAxis = ToEigen(Socket->RelativeRotation.Vector());
|
|
}
|
|
}
|
|
if (const UStaticMeshSocket* Socket = GetSocketByName("JointNext")){
|
|
if (NextJoint){
|
|
NextJoint->FirstLocalPosition = ToEigen(Socket->RelativeLocation);
|
|
NextJoint->FirstRotateAxis = ToEigen(Socket->RelativeRotation.Vector());
|
|
}
|
|
}
|
|
}
|
|
|
|
void ULink::Update(const double H){
|
|
Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass * 0;
|
|
if (IsBase)
|
|
ExtTransForce = Vector3d::Zero();
|
|
|
|
Last_Position = Position;
|
|
Velocity += H * ExtTransForce / Mass;
|
|
Position += H * Velocity;
|
|
|
|
Last_Orientation = Orientation;
|
|
// Angle Velocity has no external contributor
|
|
Vector3d w = AngularVelocity;
|
|
Orientation = Orientation + Quaterniond(0, w.x(), w.y(), w.z()) * Orientation * 0.5 * H;
|
|
Orientation.normalize();
|
|
}
|
|
|
|
void ULink::Integrate(const double H){
|
|
Velocity = (Position - Last_Position) / H;
|
|
Quaterniond DeltaOrientation = Orientation * Last_Orientation.inverse();
|
|
AngularVelocity = 2 * DeltaOrientation.vec() / H;
|
|
AngularVelocity = DeltaOrientation.w() >= 0 ? AngularVelocity : -AngularVelocity;
|
|
}
|
|
|
|
void ULink::UpdateInternalTransform(){
|
|
SetWorldLocation(FromEigen(Position));
|
|
SetWorldRotation(FromEigen(Orientation));
|
|
}
|
|
|