|
|
|
// Fill out your copyright notice in the Description page of Project Settings.
|
|
|
|
|
|
|
|
|
|
|
|
#include "Joint.h"
|
|
|
|
|
|
|
|
#include "Link.h"
|
|
|
|
#include "util.h"
|
|
|
|
#include "Kismet/KismetMathLibrary.h"
|
|
|
|
|
|
|
|
Vector3d UJoint::GetFirstWorldPosition() const{
|
|
|
|
return FirstLink->Position + FirstLink->Orientation * FirstLocalPosition;
|
|
|
|
}
|
|
|
|
|
|
|
|
Vector3d UJoint::GetSecondWorldPosition() const{
|
|
|
|
return SecondLink->Position + SecondLink->Orientation * SecondLocalPosition;
|
|
|
|
}
|
|
|
|
|
|
|
|
void UJoint::SolvePosition(const double H) const{
|
|
|
|
ULink* L1 = FirstLink;
|
|
|
|
ULink* L2 = SecondLink;
|
|
|
|
|
|
|
|
// Positional Constraints
|
|
|
|
const Vector3d R1 = GetFirstWorldPosition();
|
|
|
|
const Vector3d R2 = GetSecondWorldPosition();
|
|
|
|
const Vector3d D = R1 - R2; // Original PBD (not R2 - R1), because Impulse P is negated
|
|
|
|
const Vector3d N = D.normalized();
|
|
|
|
const double C = D.norm();
|
|
|
|
const Matrix3d I1 = L1->GetInertia();
|
|
|
|
const Matrix3d I2 = L2->GetInertia();
|
|
|
|
const double M1 = L1->Mass;
|
|
|
|
const double M2 = L2->Mass;
|
|
|
|
|
|
|
|
const double W1 = L1->GetPositionalInverseMass(R1, N);
|
|
|
|
const double W2 = L2->GetPositionalInverseMass(R2, N);
|
|
|
|
|
|
|
|
const double A = 0.000001 / (H * H);
|
|
|
|
const Vector3d P = -C / (W1 + W2 + A) * N;
|
|
|
|
|
|
|
|
L1->Position += P / M1;
|
|
|
|
L2->Position -= P / M2;
|
|
|
|
|
|
|
|
Vector3d T1 = I1.inverse() * R1.cross(P);
|
|
|
|
Vector3d T2 = I2.inverse() * R2.cross(P);
|
|
|
|
L1->Orientation += Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
|
|
|
|
L2->Orientation -= Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
|
|
|
|
|
|
|
|
// Rotational Constraints
|
|
|
|
}
|
|
|
|
|
|
|
|
void UJoint::SolveVelocity(const double H) const{
|
|
|
|
ULink* L1 = FirstLink;
|
|
|
|
ULink* L2 = SecondLink;
|
|
|
|
|
|
|
|
// Damping
|
|
|
|
const Vector3d V1 = L1->Velocity;
|
|
|
|
const Vector3d V2 = L2->Velocity;
|
|
|
|
|
|
|
|
constexpr double MuLin = 100000;
|
|
|
|
const Vector3d DeltaV = (V2 - V1) * UKismetMathLibrary::Min(MuLin * H, 1);
|
|
|
|
|
|
|
|
const Vector3d R1 = GetFirstWorldPosition();
|
|
|
|
const Vector3d R2 = GetSecondWorldPosition();
|
|
|
|
const Vector3d N = (R2 - R1).normalized();
|
|
|
|
|
|
|
|
const Matrix3d I1 = L1->GetInertia();
|
|
|
|
const Matrix3d I2 = L2->GetInertia();
|
|
|
|
|
|
|
|
const double W1 = L1->GetPositionalInverseMass(R1, N);
|
|
|
|
const double W2 = L2->GetPositionalInverseMass(R2, N);
|
|
|
|
|
|
|
|
const Vector3d P = DeltaV / (W1 + W2);
|
|
|
|
|
|
|
|
// UE_LOG(LogTemp, Log, TEXT("%f | %f"), (V2 - V1).norm(), MuLin * H);
|
|
|
|
|
|
|
|
L1->Velocity += P / L1->Mass;
|
|
|
|
L2->Velocity -= P / L2->Mass;
|
|
|
|
L1->AngularVelocity += I1.inverse() * R1.cross(P);
|
|
|
|
L2->AngularVelocity -= I2.inverse() * R2.cross(P);
|
|
|
|
}
|
|
|
|
|
|
|
|
UJoint::UJoint()
|
|
|
|
{
|
|
|
|
}
|