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.
81 lines
2.3 KiB
81 lines
2.3 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;
|
|
|
|
// Inertia tensor in world space
|
|
Matrix3d ULink::GetMomentOfInertia() const {
|
|
return GetRotationMatrix() * Inertia_Tensor_Local;
|
|
}
|
|
|
|
Matrix3d ULink::GetRotationMatrix() const{
|
|
const FQuat Q = this->GetRelativeRotation().Quaternion();
|
|
return Quaterniond(Q.W, Q.X, Q.Y, Q.Z).toRotationMatrix();
|
|
}
|
|
|
|
void ULink::Setup(){
|
|
SetupJoints();
|
|
SetupProperties();
|
|
}
|
|
|
|
void ULink::SetupProperties(){
|
|
Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
|
|
|
|
CenterOfMass = ToEigen(GetCenterOfMass());
|
|
Position = ToEigen(GetComponentLocation());
|
|
Velocity = Vector3d::Zero();
|
|
Mass = IsBase ? INFINITY : CalculateMass();
|
|
Orientation = Quaterniond::Identity();
|
|
AngularVelocity = Vector3d::Zero();
|
|
Inertia = GetMomentOfInertia();
|
|
}
|
|
|
|
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){
|
|
const Vector3d ExtTransForce = Vector3d(0, 0, -9.81 * 100) * Mass;
|
|
|
|
Last_Position = Position;
|
|
Velocity += H * ExtTransForce / Mass;
|
|
Position += H * Velocity;
|
|
|
|
Last_Orientation = Orientation;
|
|
// Angle Velocity has no external contributor
|
|
Vector3d w = AngularVelocity;
|
|
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::UpdateVisualTransform(){
|
|
SetWorldLocation(FromEigen(Position));
|
|
SetWorldRotation(FromEigen(Orientation));
|
|
}
|
|
|