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.
77 lines
1.9 KiB
77 lines
1.9 KiB
// Fill out your copyright notice in the Description page of Project Settings.
|
|
|
|
|
|
#include "Robot.h"
|
|
|
|
#include "util.h"
|
|
|
|
ARobot::ARobot(){
|
|
PrimaryActorTick.bCanEverTick = true;
|
|
}
|
|
|
|
void ARobot::BeginPlay(){
|
|
Super::BeginPlay();
|
|
|
|
for (auto* Link : Links)
|
|
Link->Setup();
|
|
ResetPositionAndOrientation();
|
|
}
|
|
|
|
void ARobot::ResetPositionAndOrientation(){
|
|
for (const auto* Joint : Joints){
|
|
const Vector3d ConnectPosL1 = Joint->FirstLink->Position + Joint->GetFirstWorldDirection();
|
|
Joint->SecondLink->Position = ConnectPosL1 - Joint->GetSecondWorldDirection();
|
|
Joint->SecondLink->Orientation = Quaterniond::Identity();
|
|
}
|
|
}
|
|
|
|
|
|
void ARobot::Connect(ULink* L1, ULink* L2){
|
|
Links.AddUnique(L1);
|
|
Links.Add(L2);
|
|
UJoint* Joint = NewObject<UJoint>(this);
|
|
Joint->FirstLink = L1;
|
|
Joint->SecondLink = L2;
|
|
L1->NextJoint = Joint;
|
|
L2->PrevJoint = Joint;
|
|
Joints.Add(Joint);
|
|
}
|
|
|
|
|
|
void ARobot::Tick(const float DeltaTime){
|
|
Super::Tick(DeltaTime);
|
|
constexpr int SubSteps = 50;
|
|
const double h = DeltaTime / SubSteps;
|
|
for (int i = 0; i < SubSteps; i++){
|
|
for (auto* Link : Links){
|
|
Vector3d TransForce = Vector3d::Zero();
|
|
Vector3d RotForce = Vector3d::Zero();
|
|
if (HasGravity)
|
|
TransForce += Link->GetGravityForce();
|
|
if (Link->IsEffector){
|
|
FVector Translation, Rotation;
|
|
GetEffectorForce(Translation, Rotation);
|
|
TransForce += Link->Mass * ToEigen(Translation);
|
|
RotForce += Link->Mass * ToEigen(Rotation);
|
|
}
|
|
Link->Update(h, TransForce, RotForce);
|
|
}
|
|
for (const auto* Joint : Joints)
|
|
Joint->SolvePosition(h);
|
|
for (auto* Link : Links)
|
|
Link->UpdateVelocity(h);
|
|
for (const auto* Joint : Joints)
|
|
Joint->SolveVelocity(h);
|
|
}
|
|
for (auto* Link : Links)
|
|
Link->UpdateInternalTransform();
|
|
}
|
|
|
|
ULink* ARobot::GetEffector() const{
|
|
for (auto* Link : Links)
|
|
if (Link->IsEffector)
|
|
return Link;
|
|
return nullptr;
|
|
}
|
|
|
|
void ARobot::GetEffectorForce_Implementation(FVector& Translation, FVector& Rotation){}
|
|
|