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.
120 lines
3.6 KiB
120 lines
3.6 KiB
// 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;
|
|
}
|
|
|
|
Vector3d UJoint::GetFirstWorldAxis() const{
|
|
return FirstLink->Orientation * FirstRotateAxis;
|
|
}
|
|
Vector3d UJoint::GetSecondWorldAxis() const{
|
|
return SecondLink->Orientation * SecondRotateAxis;
|
|
}
|
|
|
|
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);
|
|
|
|
constexpr double A = 1. / 10000000; // Compliance (inverse of stiffness)
|
|
const Vector3d P = -C / (W1 + W2 + A / (H * H)) * N;
|
|
|
|
L1->Position += P / M1;
|
|
L2->Position -= P / M2;
|
|
|
|
Vector3d T1 = I1.inverse() * R1.cross(P);
|
|
Vector3d T2 = I2.inverse() * R2.cross(P);
|
|
Quaterniond Add1 = Quaterniond(0, T1.x(), T1.y(), T1.z()) * L1->Orientation * 0.5;
|
|
Quaterniond Add2 = Quaterniond(0, T2.x(), T2.y(), T2.z()) * L2->Orientation * 0.5;
|
|
//L1->Orientation = L1->Orientation + Add1;
|
|
//L2->Orientation = L2->Orientation- Add2;
|
|
//UE_LOG(LogTemp, Log, TEXT("%f %f"), (T1).norm(), T2.norm());
|
|
}
|
|
|
|
// Rotational Constraints
|
|
{
|
|
const Vector3d A1 = GetFirstWorldAxis();
|
|
const Vector3d A2 = GetSecondWorldAxis();
|
|
const Vector3d Q = A1.cross(A2);
|
|
const Vector3d N = Q.normalized();
|
|
const double Theta = Q.norm();
|
|
|
|
const Matrix3d I1 = L1->GetInertia();
|
|
const Matrix3d I2 = L2->GetInertia();
|
|
|
|
const double W1 = L1->GetRotationalInverseMass(N);
|
|
const double W2 = L2->GetRotationalInverseMass(N);
|
|
|
|
const double A = 0.000001 / (H * H);
|
|
const Vector3d P = -Theta / (W1 + W2 + A) * N;
|
|
|
|
Vector3d T1 = I1.inverse() * P;
|
|
Vector3d T2 = I2.inverse() * P;
|
|
T1 = T2 = 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;
|
|
|
|
// UE_LOG(LogTemp, Log, TEXT("%f | %f"), W1, W2);
|
|
}
|
|
|
|
}
|
|
|
|
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; // Damping strength
|
|
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()
|
|
{
|
|
}
|
|
|