// 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);
	}

}