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.
100 lines
2.8 KiB
100 lines
2.8 KiB
// Fill out your copyright notice in the Description page of Project Settings.
|
|
|
|
|
|
#include "Link.h"
|
|
#include "util.h"
|
|
#include "Eigen/Dense"
|
|
#include "Engine/StaticMeshSocket.h"
|
|
|
|
using namespace Eigen;
|
|
|
|
Vector3d ULink::GetGravityForce() const{
|
|
if (IsBase)
|
|
return Vector3d::Zero();
|
|
return Vector3d(0, 0, -9.81 * 100) * Mass;
|
|
}
|
|
|
|
Matrix3d ULink::GetInertia() const {
|
|
const Matrix3d R = Orientation.toRotationMatrix();
|
|
return R * Inertia_Tensor_Local * R.transpose();
|
|
}
|
|
|
|
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(){
|
|
|
|
Mass = CalculateMass();
|
|
Position = ToEigen(GetComponentLocation());
|
|
Velocity = Vector3d::Zero();
|
|
Inertia_Tensor_Local = ToEigen(GetInertiaTensor()).asDiagonal();
|
|
Orientation = ToEigen(GetComponentQuat());
|
|
AngularVelocity = Vector3d::Zero();
|
|
|
|
if (IsBase){
|
|
Mass = 1e50;
|
|
Inertia_Tensor_Local = Matrix3d::Identity() * 1e50;
|
|
}
|
|
}
|
|
|
|
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());
|
|
}
|
|
}
|
|
}
|
|
|
|
FVector ULink::GetPosition() const{
|
|
return FromEigen(Position);
|
|
}
|
|
FQuat ULink::GetOrientation() const{
|
|
return FromEigen(Orientation);
|
|
}
|
|
|
|
void ULink::Update(const double H, const Vector3d& ExtTransForce, const Vector3d& ExtRotForce){
|
|
|
|
// Translation
|
|
Last_Position = Position;
|
|
Velocity += H * ExtTransForce / Mass;
|
|
Position += H * Velocity;
|
|
|
|
// Orientation
|
|
Last_Orientation = Orientation;
|
|
Vector3d w = AngularVelocity;
|
|
AngularVelocity += H * GetInertia().inverse() * (ExtRotForce - w.cross(GetInertia() * w));
|
|
w = AngularVelocity;
|
|
Orientation = Orientation + Quaterniond(0, w.x(), w.y(), w.z()) * Orientation * 0.5 * H;
|
|
Orientation.normalize();
|
|
}
|
|
|
|
void ULink::UpdateVelocity(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));
|
|
}
|
|
|