|
|
|
// Fill out your copyright notice in the Description page of Project Settings.
|
|
|
|
|
|
|
|
|
|
|
|
#include "Robot.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 ULink* L1 = Joint->FirstLink;
|
|
|
|
ULink* L2 = Joint->SecondLink;
|
|
|
|
|
|
|
|
Vector3d ConnectPosL1 = L1->Position + Joint->GetFirstWorldDirection();
|
|
|
|
const Vector3d Correction = ConnectPosL1 - Joint->GetSecondWorldDirection();
|
|
|
|
L2->Position = Correction;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
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)
|
|
|
|
Link->Update(h);
|
|
|
|
for (const auto* Joint : Joints)
|
|
|
|
Joint->SolvePosition(h);
|
|
|
|
for (auto* Link : Links)
|
|
|
|
Link->Integrate(h);
|
|
|
|
for (const auto* Joint : Joints)
|
|
|
|
Joint->SolveVelocity(h);
|
|
|
|
}
|
|
|
|
for (auto* Link : Links)
|
|
|
|
Link->UpdateInternalTransform();
|
|
|
|
}
|