Files
UnrealEngine/Engine/Source/Programs/HeadlessChaos/Private/HeadlessChaosTestJoints.cpp
2025-05-18 13:04:45 +08:00

1071 lines
39 KiB
C++

// Copyright Epic Games, Inc. All Rights Reserved.
#include "HeadlessChaos.h"
#include "HeadlessChaosTestConstraints.h"
#include "Modules/ModuleManager.h"
#include "Chaos/ParticleHandle.h"
#include "Chaos/PBDRigidsEvolution.h"
#include "Chaos/PBDRigidsEvolutionGBF.h"
#include "Chaos/PBDRigidParticles.h"
#include "Chaos/PBDJointConstraints.h"
#include "Chaos/PBDRigidDynamicSpringConstraints.h"
#include "Chaos/PBDRigidSpringConstraints.h"
#include "Chaos/Box.h"
#include "Chaos/Plane.h"
#include "Chaos/Sphere.h"
#include "Chaos/ImplicitObject.h"
#include "Chaos/ImplicitObjectTransformed.h"
#include "Chaos/ImplicitObjectUnion.h"
#include "Chaos/Levelset.h"
#include "Chaos/UniformGrid.h"
#include "Chaos/Utilities.h"
namespace ChaosTest {
using namespace Chaos;
/**
* Base class for joint constraint tests.
* Initialize the particle and joint data in the test code and call Create()
*/
template <typename TEvolution>
class FJointConstraintsTest : public FConstraintsTest<TEvolution>
{
public:
using Base = FConstraintsTest<TEvolution>;
using Base::Evolution;
using Base::AddParticleBox;
using Base::GetParticle;
FJointConstraintsTest(const int32 NumIterations, const FReal Gravity)
: Base(NumIterations, Gravity)
{
}
FPBDJointConstraintHandle* AddJoint(const TVec2<FGeometryParticleHandle*>& InConstrainedParticles, const FVec3& InLocation)
{
FPBDJointSettings SettingsTemp;
SetTestJointSetting(SettingsTemp);
FPBDJointConstraintHandle* Constraint = nullptr;
if (SettingsTemp.bUseLinearSolver)
{
Constraint = Evolution.GetJointCombinedConstraints().LinearConstraints.AddConstraint(InConstrainedParticles, FRigidTransform3(InLocation, FRotation3::FromIdentity()));
}
else
{
Constraint = Evolution.GetJointCombinedConstraints().NonLinearConstraints.AddConstraint(InConstrainedParticles, FRigidTransform3(InLocation, FRotation3::FromIdentity()));
}
FPBDJointSettings Settings = Constraint->GetSettings();
Settings.LinearProjection = 1.0;
SetTestJointSetting(Settings);
Constraint->SetSettings(Settings);
// @todo(chaos): this should be automatic, but it's handled by the proxy. Fix this
InConstrainedParticles[0]->ParticleConstraints().Add(Constraint);
InConstrainedParticles[1]->ParticleConstraints().Add(Constraint);
return Constraint;
}
virtual void Create()
{
for (int32 ParticleIndex = 0; ParticleIndex < ParticlePositions.Num(); ++ParticleIndex)
{
auto* Particle = AddParticleBox(ParticlePositions[ParticleIndex], FRotation3::MakeFromEuler(FVec3(0.f, 0.f, 0.f)).GetNormalized(), ParticleSizes[ParticleIndex], ParticleMasses[ParticleIndex]);
Evolution.EnableParticle(Particle);
}
for (int32 JointIndex = 0; JointIndex < JointPositions.Num(); ++JointIndex)
{
const TVec2<FGeometryParticleHandle*> ConstraintedParticleIds(GetParticle(JointParticleIndices[JointIndex][0]), GetParticle(JointParticleIndices[JointIndex][1]));
AddJoint(ConstraintedParticleIds, JointPositions[JointIndex]);
}
}
TFunction<void(FPBDJointSettings&)>& SetSetTestJointSetting() { return SetTestJointSetting; }
// Initial particles setup
TArray<FVec3> ParticlePositions;
TArray<FVec3> ParticleSizes;
TArray<FReal> ParticleMasses;
// Initial joints setup
TArray<FVec3> JointPositions;
TArray<TVec2<int32>> JointParticleIndices;
TFunction<void(FPBDJointSettings&)> SetTestJointSetting = [](FPBDJointSettings& InputSetting) {};
};
/**
* One kinematic, one dynamic particle connected by a ball-socket joint in the middle.
*/
template <typename TEvolution>
void JointConstraint_Single()
{
const int32 NumIterations = 1;
const FReal Gravity = 980;
FJointConstraintsTest<TEvolution> Test(NumIterations, Gravity);
Test.ParticlePositions =
{
{ (FReal)0, (FReal)0, (FReal)1000 },
{ (FReal)500, (FReal)0, (FReal)1000 },
};
Test.ParticleSizes =
{
{ (FReal)100, (FReal)100, (FReal)100 },
{ (FReal)100, (FReal)100, (FReal)100 },
};
Test.ParticleMasses =
{
(FReal)0,
(FReal)1,
};
Test.JointPositions =
{
{ (FReal)250, (FReal)0, (FReal)1000 },
};
Test.JointParticleIndices =
{
{ 0, 1 },
};
Test.SetSetTestJointSetting() = [](FPBDJointSettings& JointSettings)
{
JointSettings.bUseLinearSolver = false;
};
Test.Create();
const int32 Box1Id = 0;
const int32 Box2Id = 1;
const FReal ExpectedDistance = (Test.ParticlePositions[1] - Test.ParticlePositions[0]).Size();
const FVec3 Box2LocalSpaceJointPosition = Test.JointPositions[0] - Test.ParticlePositions[1];
const FReal Dt = 0.01f;
for (int32 i = 0; i < 100; ++i)
{
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
// Joint position calculated from pose and local-space joint pos
const FVec3 Box2WorldSpaceJointPosition = Test.GetParticle(Box2Id)->GetR().RotateVector(Box2LocalSpaceJointPosition) + Test.GetParticle(Box2Id)->GetX();
EXPECT_LT((Box2WorldSpaceJointPosition - Test.JointPositions[0]).Size(), (FReal)0.1);
// Kinematic particle should not have moved
EXPECT_LT((Test.GetParticle(Box1Id)->GetX() - Test.ParticlePositions[0]).Size(), (FReal)0.1);
}
}
template <typename TEvolution>
void JointConstraint_SingleMoveRoot()
{
const int32 NumIterations = 10;
const FReal Gravity = 0;
const FReal BoxSize = 1;
const FReal BoxMass = 1;
const FReal Dt = (FReal)1 / 20;
const FVec3 RootDelta(1 * BoxSize, 0, 0);
FJointConstraintsTest<TEvolution> Test(NumIterations, Gravity);
Test.ParticlePositions =
{
{ (FReal)0, (FReal)0, (FReal)10 * BoxSize },
{ (FReal)0, (FReal)0, (FReal)5 * BoxSize },
};
Test.ParticleSizes =
{
{ (FReal)BoxSize, (FReal)BoxSize, (FReal)BoxSize },
{ (FReal)BoxSize, (FReal)BoxSize, (FReal)BoxSize },
};
Test.ParticleMasses =
{
(FReal)0,
(FReal)BoxMass,
};
Test.JointPositions =
{
Test.ParticlePositions[0],
};
Test.JointParticleIndices =
{
{ 0, 1 },
};
Test.Create();
const int32 Box1Id = 0;
const int32 Box2Id = 1;
const FReal ExpectedDistance = (Test.ParticlePositions[1] - Test.ParticlePositions[0]).Size();
const FVec3 Box2LocalSpaceJointPosition = Test.JointPositions[0] - Test.ParticlePositions[1];
// Everything should be in a stable state
for (int32 i = 0; i < 10; ++i)
{
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
// Nothing should have moved
for (int32 ParticleIndex = 0; ParticleIndex < Test.ParticlePositions.Num(); ++ParticleIndex)
{
EXPECT_LT((Test.GetParticle(ParticleIndex)->GetX() - Test.ParticlePositions[ParticleIndex]).Size(), (FReal)0.1) << "Initial configuration instability on frame " << i;
}
}
// Move the kinematic body
const FVec3 RootPosition = Test.ParticlePositions[0] + RootDelta;
Test.Evolution.SetParticleKinematicTarget(Test.GetParticle(Box1Id)->CastToKinematicParticle(), FKinematicTarget::MakePositionTarget(RootPosition, Test.GetParticle(Box1Id)->GetR()));
for (int32 i = 0; i < 1000; ++i)
{
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
// Kinematic particle should have moved to animated position
EXPECT_LT((Test.GetParticle(Box1Id)->GetX() - RootPosition).Size(), (FReal)0.1 * BoxSize) << "Post-move instability on frame " << i;
// Particles should remain fixed distance apart (joint point is at Box1 location)
// NOTE: when using linear joints the error can be moderately large
const FVec3 Delta = Test.GetParticle(Box2Id)->GetX() - Test.GetParticle(Box1Id)->GetX();
const FReal Distance = Delta.Size();
EXPECT_NEAR(Distance, ExpectedDistance, (FReal)0.15 * BoxSize) << "Post-move instability on frame " << i;
// Joint position calculted from pose and local-space joint pos
const FVec3 Box2WorldSpaceJointPosition = Test.GetParticle(Box2Id)->GetR().RotateVector(Box2LocalSpaceJointPosition) + Test.GetParticle(Box2Id)->GetX();
EXPECT_LT((Box2WorldSpaceJointPosition - RootPosition).Size(), (FReal)0.15 * BoxSize) << "Post-move instability on frame " << i;
}
}
/**
* Pendulum with animated root.
*/
template <typename TEvolution>
void JointConstraint_SingleAnimated()
{
const int32 NumIterations = 5;
const FReal Gravity = 980;
const FReal BoxSize = 100;
const FReal BoxMass = 1000;
const FReal Dt = (FReal)1 / 20;
const FReal AnimPeriod = (FReal)2;
const FVec3 AnimDelta = FVec3(BoxSize, 0, 0);
FJointConstraintsTest<TEvolution> Test(NumIterations, Gravity);
Test.ParticlePositions =
{
{ (FReal)0, (FReal)0, (FReal)10 * BoxSize },
{ (FReal)0, (FReal)2 * BoxSize, (FReal)10 * BoxSize },
};
Test.ParticleSizes =
{
{ (FReal)BoxSize, (FReal)BoxSize, (FReal)BoxSize },
{ (FReal)BoxSize, (FReal)BoxSize, (FReal)BoxSize },
};
Test.ParticleMasses =
{
(FReal)0,
(FReal)BoxMass,
};
Test.JointPositions =
{
Test.ParticlePositions[0],
};
Test.JointParticleIndices =
{
{ 0, 1 },
};
Test.Create();
const int32 Box1Id = 0;
const int32 Box2Id = 1;
const FReal ExpectedDistance = (Test.ParticlePositions[1] - Test.ParticlePositions[0]).Size();
const FVec3 Box2LocalSpaceJointPosition = Test.JointPositions[0] - Test.ParticlePositions[1];
for (int32 i = 0; i < 1000; ++i)
{
const FReal Time = i * Dt;
const FVec3 RootOffset = FMath::Sin((FReal)2 * PI * Time / AnimPeriod) * AnimDelta;
const FVec3 RootPosition = Test.ParticlePositions[0] + RootOffset;
Test.Evolution.SetParticleKinematicTarget(Test.GetParticle(Box1Id)->CastToKinematicParticle(), FKinematicTarget::MakePositionTarget(RootPosition, Test.GetParticle(Box1Id)->GetR()));
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
// Kinematic particle should have moved to animated position
EXPECT_LT((Test.GetParticle(Box1Id)->GetX() - RootPosition).Size(), (FReal)1) << "Failed on frame " << i;
// Particles should remain fixed distance apart (joint point is at Box1 location)
const FVec3 Delta = Test.GetParticle(Box2Id)->CastToRigidParticle()->GetP() - Test.GetParticle(Box1Id)->GetX();
const FReal Distance = Delta.Size();
EXPECT_NEAR(Distance, ExpectedDistance, (FReal)1) << "Failed on frame " << i;
// Joint position calculated from pose and local-space joint pos
const FVec3 Box2WorldSpaceJointPosition = Test.GetParticle(Box2Id)->GetR().RotateVector(Box2LocalSpaceJointPosition) + Test.GetParticle(Box2Id)->GetX();
EXPECT_LT((Box2WorldSpaceJointPosition - RootPosition).Size(), (FReal)1) << "Failed on frame " << i;
}
}
/**
* Pendulum with animated root.
*/
template <typename TEvolution>
void JointConstraint_ShortChainAnimated()
{
const int32 NumIterations = 10;
const FReal Gravity = 980;
const FReal BoxSize = 100;
const FReal BoxMass = 1000;
const FReal Dt = (FReal)1 / 100;
const FReal AnimPeriod = (FReal)1;
const FVec3 AnimDelta = FVec3(5 * BoxSize, 0, 0);
const FReal AcceptableDistanceError = 5;
FJointConstraintsTest<TEvolution> Test(NumIterations, Gravity);
Test.ParticlePositions =
{
{ (FReal)0, (FReal)0, (FReal)20 * BoxSize },
{ (FReal)0, (FReal)2 * BoxSize, (FReal)20 * BoxSize },
{ (FReal)0, (FReal)4 * BoxSize, (FReal)20 * BoxSize },
};
Test.ParticleSizes =
{
{ (FReal)BoxSize, (FReal)BoxSize, (FReal)BoxSize },
{ (FReal)BoxSize, (FReal)BoxSize, (FReal)BoxSize },
{ (FReal)BoxSize, (FReal)BoxSize, (FReal)BoxSize },
};
Test.ParticleMasses =
{
(FReal)0,
(FReal)BoxMass,
(FReal)BoxMass,
};
Test.JointPositions =
{
Test.ParticlePositions[0],
Test.ParticlePositions[1],
};
Test.JointParticleIndices =
{
{ 0, 1 },
{ 1, 2 },
};
Test.Create();
const FVec3 Box2LocalSpaceJointPosition = Test.JointPositions[0] - Test.ParticlePositions[1];
FReal MaxDistanceError = 0.0f;
int32 MaxDistanceErrorFrameIndex = INDEX_NONE;
for (int32 FrameIndex = 0; FrameIndex < 1000; ++FrameIndex)
{
const FReal Time = FrameIndex * Dt;
const FVec3 RootOffset = FMath::Sin((FReal)2 * PI * Time / AnimPeriod) * AnimDelta;
const FVec3 RootPosition = Test.ParticlePositions[0] + RootOffset;
Test.Evolution.SetParticleKinematicTarget(Test.GetParticle(0)->CastToKinematicParticle(), FKinematicTarget::MakePositionTarget(RootPosition, Test.GetParticle(0)->GetR()));
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
// Particles should remain fixed distance apart
for (int JointIndex = 0; JointIndex < Test.JointPositions.Num(); ++JointIndex)
{
const int32 ParticleIndex1 = Test.JointParticleIndices[JointIndex][0];
const int32 ParticleIndex2 = Test.JointParticleIndices[JointIndex][1];
const FVec3 Delta = Test.GetParticle(ParticleIndex2)->CastToRigidParticle()->GetP() - Test.GetParticle(ParticleIndex1)->GetX();
const FReal Distance = Delta.Size();
const FReal ExpectedDistance = (Test.ParticlePositions[ParticleIndex2] - Test.ParticlePositions[ParticleIndex1]).Size();
EXPECT_NEAR(Distance, ExpectedDistance, AcceptableDistanceError) << "Joint " << JointIndex << " on frame " << FrameIndex;
const FReal DistanceError = FMath::Abs(Distance - ExpectedDistance);
if (DistanceError > MaxDistanceError)
{
MaxDistanceError = DistanceError;
MaxDistanceErrorFrameIndex = FrameIndex;
}
}
}
EXPECT_LT(MaxDistanceError, AcceptableDistanceError) << "On frame " << MaxDistanceErrorFrameIndex;
}
/**
* Pendulum with animated root.
*/
template <typename TEvolution>
void JointConstraint_LongChainAnimated()
{
const int NumParticles = 10;
const int32 NumIterations = 20;
const FReal Gravity = 980;
const FReal BoxSize = 100;
const FReal BoxMass = 1000;
const FReal Dt = (FReal)1 / 20;
const FReal AnimPeriod = (FReal)1;
const FVec3 AnimDelta = FVec3(1 * BoxSize, 0, 0);
const FReal AcceptableDistanceError = 5;
const FReal Separation = 2 * BoxSize;
const FVec3 Begin = FVec3(0, 0, (NumParticles + 10) * Separation);
const FVec3 Dir = FVec3(0, 1, 0);
const bool bRandomizeConstraintOrder = true;
FMath::RandInit(1048604845);
// Create a chain of connected particles, with particle 0 fixed
FJointConstraintsTest<TEvolution> Test(NumIterations, Gravity);
for (int32 ParticleIndex = 0; ParticleIndex < NumParticles; ++ParticleIndex)
{
Test.ParticlePositions.Add(Begin + ParticleIndex * Separation * Dir);
Test.ParticleSizes.Add({ BoxSize, BoxSize, BoxSize });
Test.ParticleMasses.Add((ParticleIndex == 0) ? (FReal)0 : BoxMass);
}
for (int32 JointIndex = 0; JointIndex < NumParticles - 1; ++JointIndex)
{
Test.JointPositions.Add(Test.ParticlePositions[JointIndex]);
Test.JointParticleIndices.Add({ JointIndex, JointIndex + 1 });
}
// Randomize constraint order
if (bRandomizeConstraintOrder)
{
for (int32 JointIndex = 0; JointIndex < Test.JointParticleIndices.Num(); ++JointIndex)
{
const int32 Index0 = JointIndex;
const int32 Index1 = FMath::RandRange(0, Test.JointParticleIndices.Num() - 1);
Test.JointPositions.Swap(Index0, Index1);
Test.JointParticleIndices.Swap(Index0, Index1);
}
}
Test.Create();
const FVec3 Box2LocalSpaceJointPosition = Test.JointPositions[0] - Test.ParticlePositions[1];
FReal MaxDistanceError = 0.0f;
int32 MaxDistanceErrorFrameIndex = INDEX_NONE;
for (int32 FrameIndex = 0; FrameIndex < 1000; ++FrameIndex)
{
const FReal Time = FrameIndex * Dt;
const FVec3 RootOffset = FMath::Sin((FReal)2 * PI * Time / AnimPeriod) * AnimDelta;
const FVec3 RootPosition = Test.ParticlePositions[0] + RootOffset;
Test.Evolution.SetParticleKinematicTarget(Test.GetParticle(0)->CastToKinematicParticle(), FKinematicTarget::MakePositionTarget(RootPosition, Test.GetParticle(0)->GetR()));
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
// Particles should remain fixed distance apart
for (int JointIndex = 0; JointIndex < Test.JointPositions.Num(); ++JointIndex)
{
const int32 ParticleIndex1 = Test.JointParticleIndices[JointIndex][0];
const int32 ParticleIndex2 = Test.JointParticleIndices[JointIndex][1];
const FVec3 Delta = Test.GetParticle(ParticleIndex2)->CastToRigidParticle()->GetP() - Test.GetParticle(ParticleIndex1)->GetX();
const FReal Distance = Delta.Size();
const FReal ExpectedDistance = (Test.ParticlePositions[ParticleIndex2] - Test.ParticlePositions[ParticleIndex1]).Size();
EXPECT_NEAR(Distance, ExpectedDistance, AcceptableDistanceError) << "Joint " << JointIndex << " on frame " << FrameIndex;
// Track largest error that exceeds threshold
const FReal DistanceError = FMath::Abs(Distance - ExpectedDistance);
if (DistanceError > MaxDistanceError)
{
MaxDistanceError = DistanceError;
MaxDistanceErrorFrameIndex = FrameIndex;
}
}
}
// Report the largest error and when it occurred if it exceeded the threshold
EXPECT_LT(MaxDistanceError, AcceptableDistanceError) << "On frame " << MaxDistanceErrorFrameIndex;
}
/**
* Pendulum with animated root with varying number of iterations.
*/
template <typename TEvolution>
void JointConstraint_LongChainVaryingIterations()
{
const int NumParticles = 10;
const int32 NumIterations = 20;
const FReal Gravity = 980;
const FReal BoxSize = 100;
const FReal BoxMass = 1000;
const FReal Dt = (FReal)1 / 20;
const FReal AcceptableDistanceError = 5;
const FReal Separation = 2 * BoxSize;
const FVec3 Begin = FVec3(0, 0, (NumParticles + 10) * Separation);
const FVec3 Dir = FVec3(0, 1, 0);
// Create a chain of connected particles, with particle 0 fixed
FJointConstraintsTest<TEvolution> Test(NumIterations, Gravity);
for (int32 ParticleIndex = 0; ParticleIndex < NumParticles; ++ParticleIndex)
{
Test.ParticlePositions.Add(Begin + ParticleIndex * Separation * Dir);
Test.ParticleSizes.Add({ BoxSize, BoxSize, BoxSize });
Test.ParticleMasses.Add((ParticleIndex == 0) ? (FReal)0 : BoxMass);
}
for (int32 JointIndex = 0; JointIndex < NumParticles - 1; ++JointIndex)
{
Test.JointPositions.Add(Test.ParticlePositions[JointIndex]);
Test.JointParticleIndices.Add({ JointIndex, JointIndex + 1 });
}
Test.Create();
const FVec3 Box2LocalSpaceJointPosition = Test.JointPositions[0] - Test.ParticlePositions[1];
FReal MaxDistanceErrorBigIterations = 0.0f;
FReal MaxDistanceErrorSmallIterations = 0.0f;
const int32 SmallPositionIterations = 1;
const int32 SmallVelocityIterations = 1;
const int32 SmallProjectionIterations = 1;
const int32 BigPositionIterations = 100;
const int32 BigVelocityIterations = 100;
const int32 BigProjectionIterations = 100;
Test.Evolution.SetNumPositionIterations(BigPositionIterations);
Test.Evolution.SetNumVelocityIterations(BigVelocityIterations);
Test.Evolution.SetNumProjectionIterations(BigProjectionIterations);
const FVec3 RootPosition = Test.ParticlePositions[0];
Test.Evolution.SetParticleKinematicTarget(Test.GetParticle(0)->CastToKinematicParticle(), FKinematicTarget::MakePositionTarget(RootPosition, Test.GetParticle(0)->GetR()));
for (int32 FrameIndex = 0; FrameIndex < 10; ++FrameIndex)
{
const FReal Time = FrameIndex * Dt;
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
Chaos::Private::FIterationSettings GroupIterations = Test.Evolution.GetIslandGroupManager().GetIslandGroupIterations(0);
EXPECT_EQ(GroupIterations.GetNumPositionIterations(), 100);
EXPECT_EQ(GroupIterations.GetNumVelocityIterations(), 100);
EXPECT_EQ(GroupIterations.GetNumProjectionIterations(), 100);
}
// Particles should remain fixed distance apart
for (int JointIndex = 0; JointIndex < Test.JointPositions.Num(); ++JointIndex)
{
const int32 ParticleIndex1 = Test.JointParticleIndices[JointIndex][0];
const int32 ParticleIndex2 = Test.JointParticleIndices[JointIndex][1];
const FVec3 Delta = Test.GetParticle(ParticleIndex2)->CastToRigidParticle()->GetP() - Test.GetParticle(ParticleIndex1)->GetX();
const FReal Distance = Delta.Size();
const FReal ExpectedDistance = (Test.ParticlePositions[ParticleIndex2] - Test.ParticlePositions[ParticleIndex1]).Size();
EXPECT_NEAR(Distance, ExpectedDistance, AcceptableDistanceError) << "Joint " << JointIndex;
// Track largest error that exceeds threshold
const FReal DistanceError = FMath::Abs(Distance - ExpectedDistance);
if (DistanceError > MaxDistanceErrorBigIterations)
{
MaxDistanceErrorBigIterations = DistanceError;
}
}
// Report the largest error and when it occurred if it exceeded the threshold
EXPECT_LT(MaxDistanceErrorBigIterations, AcceptableDistanceError);
Test.Evolution.SetNumPositionIterations(SmallPositionIterations);
Test.Evolution.SetNumVelocityIterations(SmallVelocityIterations);
Test.Evolution.SetNumProjectionIterations(SmallProjectionIterations);
for (int32 FrameIndex = 0; FrameIndex < 10; ++FrameIndex)
{
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
Chaos::Private::FIterationSettings GroupIterations = Test.Evolution.GetIslandGroupManager().GetIslandGroupIterations(0);
EXPECT_EQ(GroupIterations.GetNumPositionIterations(), 1);
EXPECT_EQ(GroupIterations.GetNumVelocityIterations(), 1);
EXPECT_EQ(GroupIterations.GetNumProjectionIterations(), 1);
}
// Check max distance error between particles
for (int JointIndex = 0; JointIndex < Test.JointPositions.Num(); ++JointIndex)
{
const int32 ParticleIndex1 = Test.JointParticleIndices[JointIndex][0];
const int32 ParticleIndex2 = Test.JointParticleIndices[JointIndex][1];
const FVec3 Delta = Test.GetParticle(ParticleIndex2)->CastToRigidParticle()->GetP() - Test.GetParticle(ParticleIndex1)->GetX();
const FReal Distance = Delta.Size();
const FReal ExpectedDistance = (Test.ParticlePositions[ParticleIndex2] - Test.ParticlePositions[ParticleIndex1]).Size();
const FReal DistanceError = FMath::Abs(Distance - ExpectedDistance);
if (DistanceError > MaxDistanceErrorSmallIterations)
{
MaxDistanceErrorSmallIterations = DistanceError;
}
}
// Report if distance error from many iterations is bigger than that from small iterations.
EXPECT_LT(MaxDistanceErrorBigIterations, MaxDistanceErrorSmallIterations);
}
/**
* Two pendulums, one solved using the linear solver, the other using nonlinear solver
*/
template <typename TEvolution>
void JointConstraint_LongChainLinearJoints()
{
const int NumParticles = 10;
const int32 NumIterations = 5;
const FReal Gravity = 980;
const FReal BoxLength = 0.01;
const FReal BoxWidth = 0.005;
const FReal BoxMass = 10;
const FReal Dt = (FReal)1 / 20;
const FReal Separation = 2 * BoxLength;
const FVec3 Begin = FVec3(0, 0, (NumParticles + 10) * Separation);
const FVec3 Dir = FVec3(0, 1, 0);
// Create two chains of connected particles, with particle 0 fixed
FJointConstraintsTest<TEvolution> LinearJointTest(NumIterations, Gravity), NonLinearJointTest(NumIterations, Gravity);
for (int32 ParticleIndex = 0; ParticleIndex < NumParticles; ++ParticleIndex)
{
LinearJointTest.ParticlePositions.Add(Begin + ParticleIndex * Separation * Dir);
LinearJointTest.ParticleSizes.Add({ BoxWidth, BoxLength, BoxWidth });
LinearJointTest.ParticleMasses.Add((ParticleIndex == 0) ? (FReal)0 : BoxMass);
NonLinearJointTest.ParticlePositions.Add(Begin + ParticleIndex * Separation * Dir);
NonLinearJointTest.ParticleSizes.Add({ BoxWidth, BoxLength, BoxWidth });
NonLinearJointTest.ParticleMasses.Add((ParticleIndex == 0) ? (FReal)0 : BoxMass);
}
for (int32 JointIndex = 0; JointIndex < NumParticles - 1; ++JointIndex)
{
LinearJointTest.JointPositions.Add(LinearJointTest.ParticlePositions[JointIndex]);
LinearJointTest.JointParticleIndices.Add({ JointIndex, JointIndex + 1 });
NonLinearJointTest.JointPositions.Add(LinearJointTest.ParticlePositions[JointIndex]);
NonLinearJointTest.JointParticleIndices.Add({ JointIndex, JointIndex + 1 });
}
LinearJointTest.SetSetTestJointSetting() = [](FPBDJointSettings& JointSettings)
{
JointSettings.bUseLinearSolver = true;
JointSettings.AngularMotionTypes = TVector<EJointMotionType, 3>(EJointMotionType::Locked);
};
NonLinearJointTest.SetSetTestJointSetting() = [](FPBDJointSettings& JointSettings)
{
JointSettings.bUseLinearSolver = false;
JointSettings.AngularMotionTypes = TVector<EJointMotionType, 3>(EJointMotionType::Locked);
};
LinearJointTest.Create();
NonLinearJointTest.Create();
for (int32 ParticleIndex = 0; ParticleIndex < NumParticles; ++ParticleIndex)
{
LinearJointTest.GetParticle(ParticleIndex)->CastToRigidParticle()->SetInertiaConditioningEnabled(false);
NonLinearJointTest.GetParticle(ParticleIndex)->CastToRigidParticle()->SetInertiaConditioningEnabled(false);
}
const FVec3 Box2LocalSpaceJointPosition = LinearJointTest.JointPositions[0] - LinearJointTest.ParticlePositions[1];
FReal MaxDistanceError = 0.0f;
int32 MaxDistanceErrorFrameIndex = INDEX_NONE;
for (int32 FrameIndex = 0; FrameIndex < 100; ++FrameIndex)
{
const FReal Time = FrameIndex * Dt;
LinearJointTest.Evolution.SetParticleKinematicTarget(LinearJointTest.GetParticle(0)->CastToKinematicParticle(), FKinematicTarget::MakePositionTarget(LinearJointTest.ParticlePositions[0], LinearJointTest.GetParticle(0)->GetR()));
LinearJointTest.Evolution.AdvanceOneTimeStep(Dt);
LinearJointTest.Evolution.EndFrame(Dt);
NonLinearJointTest.Evolution.SetParticleKinematicTarget(NonLinearJointTest.GetParticle(0)->CastToKinematicParticle(), FKinematicTarget::MakePositionTarget(NonLinearJointTest.ParticlePositions[0], NonLinearJointTest.GetParticle(0)->GetR()));
NonLinearJointTest.Evolution.AdvanceOneTimeStep(Dt);
NonLinearJointTest.Evolution.EndFrame(Dt);
}
// The chain solved using the nonlinear solver should have less error than the one using linear solver.
for (int JointIndex = 0; JointIndex < LinearJointTest.JointPositions.Num(); ++JointIndex)
{
const int32 ParticleIndex1 = LinearJointTest.JointParticleIndices[JointIndex][0];
const int32 ParticleIndex2 = LinearJointTest.JointParticleIndices[JointIndex][1];
const FVec3 LinearDelta = LinearJointTest.GetParticle(ParticleIndex2)->CastToRigidParticle()->GetP() - LinearJointTest.GetParticle(ParticleIndex1)->GetX();
const FReal LinearDistance = LinearDelta.Size();
const FVec3 NonLinearDelta = NonLinearJointTest.GetParticle(ParticleIndex2)->CastToRigidParticle()->GetP() - NonLinearJointTest.GetParticle(ParticleIndex1)->GetX();
const FReal NonLinearDistance = NonLinearDelta.Size();
const FReal ExpectedDistance = (LinearJointTest.ParticlePositions[ParticleIndex2] - LinearJointTest.ParticlePositions[ParticleIndex1]).Size();
const FReal LinearDistanceError = FMath::Abs(ExpectedDistance - LinearDistance);
const FReal NonLinearDistanceError = FMath::Abs(ExpectedDistance - NonLinearDistance);
EXPECT_LT(NonLinearDistanceError * 10, LinearDistanceError);
}
}
template <typename TEvolution>
void SpringConstraint()
{
TUniquePtr<FChaosPhysicsMaterial> PhysicalMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicalMaterial->Friction = 0;
PhysicalMaterial->Restitution = 0;
PhysicalMaterial->SleepingLinearThreshold = 0;
PhysicalMaterial->SleepingAngularThreshold = 0;
PhysicalMaterial->DisabledLinearThreshold = 0;
PhysicalMaterial->DisabledAngularThreshold = 0;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
auto StaticBox = AppendStaticParticleBox(Particles, FVec3((FReal)100, (FReal)100, (FReal)100));
auto Box2 = AppendDynamicParticleBox(Particles, FVec3((FReal)100, (FReal)100, (FReal)100));
StaticBox->SetX(FVec3((FReal)0, (FReal)0, (FReal)1000));
Box2->SetX(FVec3((FReal)500, (FReal)0, (FReal)1000));
Box2->SetP(Box2->GetX());
THandleArray<FChaosPhysicsMaterial> PhysicalMaterials;
TEvolution Evolution(Particles, PhysicalMaterials);
TVec2<FGeometryParticleHandle*> ConstrainedParticles = TVec2<FGeometryParticleHandle*>(StaticBox, Box2);
TVec2<FVec3> Points = { FVec3((FReal)100, (FReal)0, (FReal)1000), FVec3((FReal)400, (FReal)0, (FReal)1000) };
Evolution.SetPhysicsMaterial(StaticBox, MakeSerializable(PhysicalMaterial));
Evolution.SetPhysicsMaterial(Box2, MakeSerializable(PhysicalMaterial));
Evolution.EnableParticle(StaticBox);
Evolution.EnableParticle(Box2);
FPBDRigidSpringConstraints JointConstraints;
JointConstraints.AddConstraint(ConstrainedParticles, Points, 1.0f, 0.0f, (Points[0] - Points[1]).Size());
Evolution.AddConstraintContainer(JointConstraints);
const FReal Dt = 0.01f;
for (int32 i = 0; i < 100; ++i)
{
Evolution.AdvanceOneTimeStep(Dt);
Evolution.EndFrame(Dt);
EXPECT_LT(FMath::Abs((Box2->GetR().RotateVector(FVec3((FReal)-100, (FReal)0, (FReal)0)) + Box2->GetX() - Points[0]).Size() - 300.f), 0.1);
}
}
template <typename TEvolution>
void DynamicSpringConstraint()
{
TUniquePtr<FChaosPhysicsMaterial> PhysicalMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicalMaterial->Friction = 0;
PhysicalMaterial->Restitution = 0;
PhysicalMaterial->SleepingLinearThreshold = 0;
PhysicalMaterial->SleepingAngularThreshold = 0;
PhysicalMaterial->DisabledLinearThreshold = 0;
PhysicalMaterial->DisabledAngularThreshold = 0;
PhysicalMaterial->SleepCounterThreshold = 20;
{
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
auto& StaticBox = *AppendStaticParticleBox(Particles, FVec3((FReal)100, (FReal)100, (FReal)100));
auto& Box2 = *AppendDynamicParticleBox(Particles, FVec3((FReal)100, (FReal)100, (FReal)100));
StaticBox.SetX(FVec3((FReal)0, (FReal)0, (FReal)500));
Box2.SetX(FVec3((FReal)500, (FReal)0, (FReal)1000));
Box2.SetP(Box2.GetX());
THandleArray<FChaosPhysicsMaterial> PhysicalMaterials;
TEvolution Evolution(Particles, PhysicalMaterials);
TArray<TVec2<FGeometryParticleHandle*>> Constraints = { TVec2<FGeometryParticleHandle*>(&StaticBox, &Box2) };
Evolution.SetPhysicsMaterial(&StaticBox, MakeSerializable(PhysicalMaterial));
Evolution.SetPhysicsMaterial(&Box2, MakeSerializable(PhysicalMaterial));
Evolution.EnableParticle(&StaticBox);
Evolution.EnableParticle(&Box2);
Chaos::FPBDRigidDynamicSpringConstraints SpringConstraints(MoveTemp(Constraints));
Evolution.AddConstraintContainer(SpringConstraints);
const FReal Dt = 0.01f;
for (int32 i = 0; i < 200; ++i)
{
Evolution.AdvanceOneTimeStep(Dt);
Evolution.EndFrame(Dt);
}
EXPECT_LT(Box2.GetX()[2], 0);
}
{
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
auto& StaticBox = *AppendStaticParticleBox(Particles, FVec3((FReal)100, (FReal)100, (FReal)100));
auto& Box2 = *AppendDynamicParticleBox(Particles, FVec3((FReal)100, (FReal)100, (FReal)100));
StaticBox.SetX(FVec3((FReal)0, (FReal)0, (FReal)500));
Box2.SetX(FVec3((FReal)500, (FReal)0, (FReal)1000));
Box2.SetP(Box2.GetX());
THandleArray<FChaosPhysicsMaterial> PhysicalMaterials;
TEvolution Evolution(Particles, PhysicalMaterials);
TArray<TVec2<FGeometryParticleHandle*>> Constraints = { TVec2<FGeometryParticleHandle*>(&StaticBox, &Box2) };
Evolution.SetPhysicsMaterial(&StaticBox, MakeSerializable(PhysicalMaterial));
Evolution.SetPhysicsMaterial(&Box2, MakeSerializable(PhysicalMaterial));
Evolution.EnableParticle(&StaticBox);
Evolution.EnableParticle(&Box2);
Chaos::FPBDRigidDynamicSpringConstraints SpringConstraints(MoveTemp(Constraints), 400);
Evolution.AddConstraintContainer(SpringConstraints);
const FReal Dt = 0.01f;
for (int32 i = 0; i < 200; ++i)
{
Evolution.AdvanceOneTimeStep(Dt);
Evolution.EndFrame(Dt);
}
EXPECT_GT(Box2.GetX()[2], 0);
}
}
// check that joints don't simulate if a constrained particle is disabled
template <typename TEvolution>
void JointConstraint_DisableOneConstrainedParticle()
{
const int32 NumIterations = 1;
const FReal Gravity = 980;
FJointConstraintsTest<TEvolution> Test(NumIterations, Gravity);
Test.ParticlePositions = {
{ (FReal)0, (FReal)0, (FReal)1000 },
{ (FReal)500, (FReal)0, (FReal)1000 },
};
Test.ParticleSizes =
{
{ (FReal)100, (FReal)100, (FReal)100 },
{ (FReal)100, (FReal)100, (FReal)100 },
};
Test.ParticleMasses =
{
(FReal)1,
(FReal)1,
};
Test.JointPositions =
{
{ (FReal)250, (FReal)0, (FReal)1000 },
};
Test.JointParticleIndices =
{
{ 0, 1 },
};
Test.Create();
const int32 Box1Id = 0;
const int32 Box2Id = 1;
const FReal ExpectedDistance = (Test.ParticlePositions[1] - Test.ParticlePositions[0]).Size();
const FVec3 Box2LocalSpaceJointPosition = Test.JointPositions[0] - Test.ParticlePositions[1];
// box 1 disabled
Test.Evolution.DisableParticle(Test.GetParticle(Box1Id));
const FReal Dt = 0.01f;
for (int32 i = 0; i < 100; ++i)
{
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
// box 1 not simulating so would expect to not have moved
EXPECT_LT(FMath::Abs(Test.ParticlePositions[0].X - Test.GetParticle(Box1Id)->GetX().X), (FReal)0.1);
EXPECT_LT(FMath::Abs(Test.ParticlePositions[0].Y - Test.GetParticle(Box1Id)->GetX().Y), (FReal)0.1);
EXPECT_LT(FMath::Abs(Test.ParticlePositions[0].Z - Test.GetParticle(Box1Id)->GetX().Z), (FReal)0.09);
// box 2 should fall under gravity & not have moved in X or Y, constraint should not 'Apply' if other particle is disabled
EXPECT_LT(FMath::Abs(Test.ParticlePositions[1].X - Test.GetParticle(Box2Id)->GetX().X), (FReal)0.1);
EXPECT_LT(FMath::Abs(Test.ParticlePositions[1].Y - Test.GetParticle(Box2Id)->GetX().Y), (FReal)0.1);
EXPECT_GT(FMath::Abs(Test.ParticlePositions[1].Z - Test.GetParticle(Box2Id)->GetX().Z), (FReal)0.09);
}
}
GTEST_TEST(AllEvolutions, JointTests_TestSingleConstraint) {
JointConstraint_Single<FPBDRigidsEvolutionGBF>();
}
GTEST_TEST(AllEvolutions, JointTests_TestSingleConstraintWithLateralTranslation) {
JointConstraint_SingleMoveRoot<FPBDRigidsEvolutionGBF>();
}
GTEST_TEST(AllEvolutions, JointTests_TestSingleConstraintWithAnimatedRoot) {
JointConstraint_SingleAnimated<FPBDRigidsEvolutionGBF>();
}
GTEST_TEST(AllEvolutions, JointTests_TestShortJointChainWithAnimatedRoot) {
JointConstraint_ShortChainAnimated<FPBDRigidsEvolutionGBF>();
}
GTEST_TEST(AllEvolutions, JointTests_TestLongJointChainWithAnimatedRoot) {
JointConstraint_LongChainAnimated<FPBDRigidsEvolutionGBF>();
}
GTEST_TEST(AllEvolutions, JointTests_TestLongJointChainWithVaryingIterations) {
JointConstraint_LongChainVaryingIterations<FPBDRigidsEvolutionGBF>();
}
GTEST_TEST(AllEvolutions, JointTests_TestLongJointChainWithNonLinearSolvers) {
JointConstraint_LongChainLinearJoints<FPBDRigidsEvolutionGBF>();
}
GTEST_TEST(AllEvolutions, JointTests_TestSingleSpringConstraint) {
SpringConstraint<FPBDRigidsEvolutionGBF>();
}
GTEST_TEST(AllEvolutions, JointTests_TestSingleDynamicSpringConstraint) {
DynamicSpringConstraint<FPBDRigidsEvolutionGBF>();
}
GTEST_TEST(AllEvolutions, JointConstraint_TestDisableOneConstrainedParticle) {
JointConstraint_DisableOneConstrainedParticle<FPBDRigidsEvolutionGBF>();
}
// Create a kinematic-dynamic particle chain with a center of mass offset on the kinematic
// and verify that the joint offsets are used correctly. The two particles are arranged vertically
// with the joint between them, the center of mass offset should not affect behaviour.
//
// NOTE: We create both particles as dynamics and then change one to be kinematic so that we
// can alter its mass properties.
//
GTEST_TEST(JointTests, TestJointCoMOffset)
{
const int32 NumIterations = 1;
const FReal Gravity = 980;
FJointConstraintsTest<FPBDRigidsEvolutionGBF> Test(NumIterations, Gravity);
Test.ParticlePositions =
{
{ (FReal)0, (FReal)0, (FReal)0 },
{ (FReal)0, (FReal)0, (FReal)1000 },
};
Test.ParticleSizes =
{
{ (FReal)100, (FReal)100, (FReal)100 },
{ (FReal)100, (FReal)100, (FReal)100 },
};
Test.ParticleMasses =
{
(FReal)1000,
(FReal)1000,
};
Test.JointPositions =
{
{ (FReal)0, (FReal)0, (FReal)500 },
};
Test.JointParticleIndices =
{
{ 0, 1 },
};
Test.Create();
// Move the center of mass of the soon-to-be kinematic
Test.GetParticle(0)->CastToRigidParticle()->SetCenterOfMass(FVec3(0, 0, -100));
EXPECT_NEAR((Test.GetParticle(0)->CastToRigidParticle()->CenterOfMass() - FVec3(0, 0, -100)).Size(), 0, UE_KINDA_SMALL_NUMBER);
// Make the root body kinematic
Test.Evolution.SetParticleObjectState(Test.GetParticle(0)->CastToRigidParticle(), EObjectStateType::Kinematic);
// The kinematic will now report zero center of mass via the GenericParticle API, but internally it will still be set
EXPECT_NEAR(FGenericParticleHandle(Test.GetParticle(0))->CenterOfMass().Size(), 0, UE_KINDA_SMALL_NUMBER);
EXPECT_NEAR((Test.GetParticle(0)->CastToRigidParticle()->CenterOfMass() - FVec3(0, 0, -100)).Size(), 0, UE_KINDA_SMALL_NUMBER);
const FReal Dt = 0.01f;
for (int32 i = 0; i < 100; ++i)
{
Test.Evolution.AdvanceOneTimeStep(Dt);
Test.Evolution.EndFrame(Dt);
// Neither particle should have moved
EXPECT_LT((Test.GetParticle(0)->GetX() - Test.ParticlePositions[0]).Size(), (FReal)0.1);
EXPECT_LT((Test.GetParticle(1)->GetX() - Test.ParticlePositions[1]).Size(), (FReal)0.1);
}
}
// Check that constraints end up in the same island when graph is fully connected
GTEST_TEST(JointTests, TestJointConstraintGraph_Connected)
{
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs ParticleContainer(UniqueIndices);
FPBDJointConstraints JointContainer;
JointContainer.SetSortEnabled(true);
// Create 3 particles
TArray<FPBDRigidParticleHandle*> Rigids = ParticleContainer.CreateDynamicParticles(3);
// Connect particles with 2 joints
TArray<FPBDJointConstraintHandle*> Joints =
{
JointContainer.AddConstraint({ Rigids[0], Rigids[1] }, { FRigidTransform3(), FRigidTransform3() }),
JointContainer.AddConstraint({ Rigids[0], Rigids[2] }, { FRigidTransform3(), FRigidTransform3() })
};
// This sets up the joint container, including generating islands etc
JointContainer.PrepareTick();
// Both joints should be in an island
EXPECT_GE(JointContainer.GetConstraintIsland(0), 0);
EXPECT_GE(JointContainer.GetConstraintIsland(1), 0);
// Both joints should be in same island
EXPECT_EQ(JointContainer.GetConstraintIsland(0), JointContainer.GetConstraintIsland(1));
// Joints should have different colors
EXPECT_NE(JointContainer.GetConstraintColor(0), JointContainer.GetConstraintColor(1));
}
// Check that constraints islands are not merged through shared kinematic particles
GTEST_TEST(JointTests, TestJointConstraintGraph_NotConnected)
{
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs ParticleContainer(UniqueIndices);
FPBDJointConstraints JointContainer;
JointContainer.SetSortEnabled(true);
// Create 3 particles
TArray<FPBDRigidParticleHandle*> Rigids = ParticleContainer.CreateDynamicParticles(3);
// Connect particles with 2 joints
TArray<FPBDJointConstraintHandle*> Joints =
{
JointContainer.AddConstraint({ Rigids[0], Rigids[1] }, { FRigidTransform3(), FRigidTransform3() }),
JointContainer.AddConstraint({ Rigids[0], Rigids[2] }, { FRigidTransform3(), FRigidTransform3() })
};
// Set the particle in the middle of the two joints to kinematic
Rigids[0]->SetObjectStateLowLevel(EObjectStateType::Kinematic);
// This sets up the joint container, including generating islands etc
JointContainer.PrepareTick();
// Both joints should be in an island
EXPECT_GE(JointContainer.GetConstraintIsland(0), 0);
EXPECT_GE(JointContainer.GetConstraintIsland(1), 0);
// Joints should be in different islands
EXPECT_NE(JointContainer.GetConstraintIsland(0), JointContainer.GetConstraintIsland(1));
// Both joints should be at level 0
EXPECT_EQ(JointContainer.GetConstraintLevel(0), 0);
EXPECT_EQ(JointContainer.GetConstraintLevel(1), 0);
}
}