// Copyright Epic Games, Inc. All Rights Reserved.
#pragma once

#include "Chaos/Box.h"
#include "Chaos/Capsule.h"
#include "Chaos/Convex.h"
#include "Chaos/ImplicitObject.h"
#include "Chaos/ImplicitObjectTransformed.h"
#include "Chaos/ImplicitObjectUnion.h"
#include "Chaos/Levelset.h"
#include "Chaos/MassProperties.h"
#include "Chaos/Sphere.h"
#include "Chaos/TaperedCylinder.h"
#include "GeometryCollection/GeometryCollectionSimulationTypes.h"

/**
 * A collection of analytic implicit shapes parented to a single transform, 
 * in a hierarchy of transforms.
 *
 * Currently we use this class to represent bones even if they don't have any 
 * implicit shapes.
 */
class FAnalyticImplicitGroup
{
public:
	FAnalyticImplicitGroup()
		: BoneName(NAME_None)
		, BoneIndex(INDEX_NONE)
		, ParentBoneIndex(INDEX_NONE)
		, RigidBodyId(INDEX_NONE)
		, RigidBodyState(EObjectStateTypeEnum::Chaos_Object_Kinematic)
		, Parent(nullptr)
	{}
	FAnalyticImplicitGroup(const FName &InBoneName, const int32 InBoneIndex)
		: BoneName(InBoneName)
		, BoneIndex(InBoneIndex)
		, ParentBoneIndex(INDEX_NONE)
		, RigidBodyId(INDEX_NONE)
		, RigidBodyState(EObjectStateTypeEnum::Chaos_Object_Kinematic)
		, Parent(nullptr)
	{}
	FAnalyticImplicitGroup(const FAnalyticImplicitGroup &) = delete;
	FAnalyticImplicitGroup(FAnalyticImplicitGroup &&Other)
		: BoneName(MoveTemp(Other.BoneName))
		, BoneIndex(MoveTemp(Other.BoneIndex))
		, ParentBoneIndex(MoveTemp(Other.ParentBoneIndex))

		, RigidBodyId(MoveTemp(Other.RigidBodyId))
		, RigidBodyState(MoveTemp(Other.RigidBodyState))

		, Spheres(MoveTemp(Other.Spheres))
		, Boxes(MoveTemp(Other.Boxes))
		, Capsules(MoveTemp(Other.Capsules))
		, TaperedCylinders(MoveTemp(Other.TaperedCylinders))
		, ConvexHulls(MoveTemp(Other.ConvexHulls))
		, LevelSets(MoveTemp(Other.LevelSets))

		, Transforms(MoveTemp(Other.Transforms))
		, Parent(MoveTemp(Other.Parent))
		, Children(MoveTemp(Other.Children))
	{}
	~FAnalyticImplicitGroup()
	{
		for (Chaos::FSphere* Sphere : Spheres) if (Sphere) delete Sphere;
		for (Chaos::TBox<Chaos::FReal, 3>* Box : Boxes) if (Box) delete Box;
		for (Chaos::FCapsule* Capsule : Capsules) if (Capsule) delete Capsule;
		for (Chaos::FTaperedCylinder* TaperedCylinder : TaperedCylinders) if (TaperedCylinder) delete TaperedCylinder;
		for (Chaos::FConvex* ConvexHull : ConvexHulls) if (ConvexHull) delete ConvexHull;
		for (Chaos::FLevelSet* LevelSet : LevelSets) if (LevelSet) delete LevelSet;
	}

	void Init(const int32 NumStructures, const bool DoCollGeom=true)
	{
		Transforms.Reserve(NumStructures);
		if (DoCollGeom)
		{
			CollisionPoints.SetNum(NumStructures);
			CollisionTriangles.SetNum(NumStructures);
		}
	}

	/** The number of analytic shapes in this group. */
	int32 NumStructures() const { return Transforms.Num(); }

	bool IsValid() { return BoneIndex != INDEX_NONE; }

	const FName& GetBoneName() const { return BoneName; }
	const int32 GetBoneIndex() const { return BoneIndex; }

	void SetParentBoneIndex(const int32 InParentBoneIndex) { ParentBoneIndex = InParentBoneIndex; }
	const int32 GetParentBoneIndex() const { return ParentBoneIndex; }

	void SetRigidBodyId(const int32 InRigidBodyId) { RigidBodyId = InRigidBodyId; }
	int32 GetRigidBodyId() const { return RigidBodyId; }

	void SetRigidBodyState(const EObjectStateTypeEnum State) { RigidBodyState = State; }
	EObjectStateTypeEnum GetRigidBodyState() const { return RigidBodyState; }

	int32 Add(const FTransform &InitialXf, Chaos::FSphere *Sphere) { Spheres.Add(Sphere); return Transforms.Insert(InitialXf, Spheres.Num()-1); }
	int32 Add(const FTransform &InitialXf, Chaos::TBox<Chaos::FReal, 3> *Box) { Boxes.Add(Box); return Transforms.Insert(InitialXf, Spheres.Num()+Boxes.Num()-1); }
	int32 Add(const FTransform &InitialXf, Chaos::FCapsule *Capsule) { Capsules.Add(Capsule); return Transforms.Insert(InitialXf, Spheres.Num()+Boxes.Num()+Capsules.Num()-1); }
	int32 Add(const FTransform &InitialXf, Chaos::FTaperedCylinder *TaperedCylinder) { TaperedCylinders.Add(TaperedCylinder); return Transforms.Insert(InitialXf, Spheres.Num()+Boxes.Num()+Capsules.Num()+TaperedCylinders.Num()-1); }
	int32 Add(const FTransform &InitialXf, Chaos::FConvex *ConvexHull) { ConvexHulls.Add(ConvexHull); return Transforms.Insert(InitialXf, Spheres.Num()+Boxes.Num()+Capsules.Num()+TaperedCylinders.Num()+ConvexHulls.Num()-1); }
	int32 Add(const FTransform &InitialXf, Chaos::FLevelSet *LevelSet) { LevelSets.Add(LevelSet); return Transforms.Add(InitialXf); }

	void SetCollisionTopology(
		const int32 Index,
		TArray<Chaos::FVec3>&& Points,
		TArray<Chaos::TVec3<int32>>&& Triangles)
	{ CollisionPoints[Index] = MoveTemp(Points); CollisionTriangles[Index] = MoveTemp(Triangles); }

	const TArray<FTransform>& GetInitialStructureTransforms() const { return Transforms; }
	void ResetTransforms() { Transforms.Init(FTransform::Identity, Transforms.Num()); }

	Chaos::FMassProperties BuildMassProperties(
		const Chaos::FReal Density,
		Chaos::FReal& TotalMass)
	{
		// Make sure this function is being called before we've transfered ownership 
		// of our implicit shapes to the simulator.
		checkSlow(!Spheres.Contains(nullptr) && !Boxes.Contains(nullptr) && !Capsules.Contains(nullptr) && 
			!TaperedCylinders.Contains(nullptr) && !ConvexHulls.Contains(nullptr) && !LevelSets.Contains(nullptr));

		const int32 Num = NumStructures();
		TArray<Chaos::FMassProperties> MPArray;
		TArray<Chaos::FAABB3> BBoxes;
		MPArray.SetNum(Num);
		BBoxes.SetNum(Num);

		int32 TransformIndex = 0;
		for (Chaos::FSphere* Sphere : Spheres)
		{
			const FTransform& Xf = Transforms[TransformIndex];
			BBoxes[TransformIndex] = Sphere->BoundingBox().TransformedAABB(Xf);
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			MP.Volume = Sphere->GetVolume();
			MP.CenterOfMass = Xf.TransformPositionNoScale(Sphere->GetCenterOfMass());
			MP.RotationOfMass = Xf.TransformRotation(Sphere->GetRotationOfMass());
		}
		for (Chaos::TBox<Chaos::FReal, 3>* Box : Boxes)
		{
			const FTransform& Xf = Transforms[TransformIndex];
			BBoxes[TransformIndex] = Box->BoundingBox().TransformedAABB(Xf);
			Chaos::FMassProperties&MP = MPArray[TransformIndex++];
			MP.Volume = Box->GetVolume();
			MP.CenterOfMass = Xf.TransformPositionNoScale(Box->GetCenterOfMass());
			MP.RotationOfMass = Xf.TransformRotation(Box->GetRotationOfMass());
		}
		for (Chaos::FCapsule* Capsule : Capsules)
		{
			const FTransform& Xf = Transforms[TransformIndex];
			BBoxes[TransformIndex] = Capsule->BoundingBox().TransformedAABB(Xf);
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			MP.Volume = Capsule->GetVolume();
			MP.CenterOfMass = Xf.TransformPositionNoScale(FVector3d(Capsule->GetCenterOfMassf()));
			MP.RotationOfMass = Xf.TransformRotation(Capsule->GetRotationOfMass());
		}
		for (Chaos::FTaperedCylinder* TaperedCylinder : TaperedCylinders)
		{
			const FTransform& Xf = Transforms[TransformIndex];
			BBoxes[TransformIndex] = TaperedCylinder->BoundingBox().TransformedAABB(Xf);
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			MP.Volume = TaperedCylinder->GetVolume();
			MP.CenterOfMass = Xf.TransformPositionNoScale(TaperedCylinder->GetCenterOfMass());
			MP.RotationOfMass = Xf.TransformRotation(TaperedCylinder->GetRotationOfMass());
		}
		for (Chaos::FConvex* Convex : ConvexHulls)
		{
			const FTransform& Xf = Transforms[TransformIndex];
			BBoxes[TransformIndex] = Convex->BoundingBox().TransformedAABB(Xf);
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			MP.Volume = Convex->BoundingBox().GetVolume();
			MP.CenterOfMass = Xf.TransformPositionNoScale(Convex->BoundingBox().Center());
			MP.RotationOfMass = Xf.TransformRotation(Convex->BoundingBox().GetRotationOfMass());
		}
		for (Chaos::FLevelSet* LevelSet : LevelSets)
		{
			const FTransform& Xf = Transforms[TransformIndex];
			BBoxes[TransformIndex] = LevelSet->BoundingBox().TransformedAABB(Xf);
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			MP.Volume = LevelSet->BoundingBox().GetVolume();
			MP.CenterOfMass = Xf.TransformPositionNoScale(LevelSet->BoundingBox().Center());
			MP.RotationOfMass = Xf.TransformRotation(LevelSet->BoundingBox().GetRotationOfMass());
		}

		// Find overlap and adjust volumes accordingly.  We do this by determining 
		// how much their AABB's overlap for sake of speed and simplicity.  Ideally
		// we'd do something more accurate...
		for (int32 i=0; i < Num-1; i++)
		{
			const Chaos::FAABB3& BoxI = BBoxes[i];
			for (int32 j = i+1; j < Num; j++)
			{
				const Chaos::FAABB3& BoxJ = BBoxes[j];
				if (BoxI.Intersects(BoxJ))
				{
					Chaos::FAABB3 BoxIJ = BoxI.GetIntersection(BoxJ);
					const Chaos::FReal VolIJ = BoxIJ.GetVolume();
					if (VolIJ > UE_KINDA_SMALL_NUMBER)
					{
						const Chaos::FReal VolI = BoxI.GetVolume();
						const Chaos::FReal VolJ = BoxJ.GetVolume();
						const Chaos::FReal PctOverlapI = VolI > UE_KINDA_SMALL_NUMBER ? VolIJ / VolI : 0.f;
						const Chaos::FReal PctOverlapJ = VolJ > UE_KINDA_SMALL_NUMBER ? VolIJ / VolJ : 0.f;
						// Split the overlapping volume between i and j
						MPArray[i].Volume *= static_cast<Chaos::FReal>(1.0 - PctOverlapI / 2.0);
						MPArray[j].Volume *= static_cast<Chaos::FReal>(1.0 - PctOverlapJ / 2.0);
					}
				}
			}
		}

		TotalMass = 0.f;
		TransformIndex = 0;
		for (Chaos::FSphere* Sphere : Spheres)
		{
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			Chaos::FReal Mass = Density * MP.Volume;
			TotalMass += Mass;
			MP.InertiaTensor = Sphere->GetInertiaTensor(Mass);
		}
		for (Chaos::TBox<Chaos::FReal, 3>* Box : Boxes)
		{
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			Chaos::FReal Mass = Density * MP.Volume;
			TotalMass += Mass;
			MP.InertiaTensor = Box->GetInertiaTensor(Mass);
		}
		for (Chaos::FCapsule* Capsule : Capsules)
		{
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			Chaos::FReal Mass = Density * MP.Volume;
			TotalMass += Mass;
			MP.InertiaTensor = Capsule->GetInertiaTensor(Mass);
		}
		for (Chaos::FTaperedCylinder* TaperedCylinder : TaperedCylinders)
		{
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			Chaos::FReal Mass = Density * MP.Volume;
			TotalMass += Mass;
			MP.InertiaTensor = TaperedCylinder->GetInertiaTensor(Mass);
		}
		for (Chaos::FConvex* Convex : ConvexHulls)
		{
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			Chaos::FReal Mass = Density * MP.Volume;
			TotalMass += Mass;
			MP.InertiaTensor = Convex->BoundingBox().GetInertiaTensor(Mass);
		}
		for (Chaos::FLevelSet* LevelSet : LevelSets)
		{
			Chaos::FMassProperties &MP = MPArray[TransformIndex++];
			Chaos::FReal Mass = Density * MP.Volume;
			TotalMass += Mass;
			MP.InertiaTensor = LevelSet->BoundingBox().GetInertiaTensor(Mass);
		}

		return Chaos::Combine(MPArray);
	}

	TArray<Chaos::FVec3>* BuildSamplePoints(
		const Chaos::FReal ParticlesPerUnitArea,
		const int32 MinParticles,
		const int32 MaxParticles)
	{
		// Make sure this function is being called before we've transfered ownership 
		// of our implicit shapes to the simulator.
		checkSlow(!Spheres.Contains(nullptr) && !Boxes.Contains(nullptr) && !Capsules.Contains(nullptr) && 
			!TaperedCylinders.Contains(nullptr) && !ConvexHulls.Contains(nullptr) && !LevelSets.Contains(nullptr));

		ContiguousCollisionPoints.Reset();
		const int32 Num = NumStructures();
		if (Num == 0)
		{
			return &ContiguousCollisionPoints;
		}

		int32 TransformIndex = 0;
		for (Chaos::FSphere* Sphere : Spheres)
		{
			TArray<Chaos::FVec3>& Points = CollisionPoints[TransformIndex];
			if (!Points.Num())
				Points = Sphere->ComputeSamplePoints(ParticlesPerUnitArea, MinParticles, MaxParticles);
			CullDeepPoints(Points, TransformIndex);
			TransformIndex++;
		}
		for (Chaos::TBox<Chaos::FReal, 3>* Box : Boxes)
		{
			TArray<Chaos::FVec3>& Points = CollisionPoints[TransformIndex];
			if (!Points.Num())
				Points = Box->ComputeSamplePoints();
			CullDeepPoints(Points, TransformIndex);
			TransformIndex++;
		}
		for (Chaos::FCapsule* Capsule : Capsules)
		{
			TArray<Chaos::FVec3>& Points = CollisionPoints[TransformIndex];
			if (!Points.Num())
				Points = Capsule->ComputeSamplePoints(ParticlesPerUnitArea, MinParticles, MaxParticles);
			CullDeepPoints(Points, TransformIndex);
			TransformIndex++;
		}
		for (Chaos::FTaperedCylinder* TaperedCylinder : TaperedCylinders)
		{
			TArray<Chaos::FVec3>& Points = CollisionPoints[TransformIndex];
			if (!Points.Num())
				Points = TaperedCylinder->ComputeSamplePoints(ParticlesPerUnitArea, false, MinParticles, MaxParticles);
			CullDeepPoints(Points, TransformIndex);
			TransformIndex++;
		}
		for (Chaos::FConvex* Convex : ConvexHulls)
		{
			TArray<Chaos::FVec3>& Points = CollisionPoints[TransformIndex];
			if (!Points.Num())
			{
				const Chaos::FAABB3& BBox = Convex->BoundingBox();
				Chaos::FSphere Sphere(BBox.Center(), BBox.Extents().Size() / 2);
				Points = Sphere.ComputeSamplePoints(ParticlesPerUnitArea, MinParticles, MaxParticles);
				Chaos::FVec3 Normal;
				for (Chaos::FVec3 &Pt : Points)
				{
					const Chaos::FReal Phi = Convex->PhiWithNormal(Pt, Normal);
					Pt += Normal * -Phi;
					//check(FMath::Abs(Convex->SignedDistance(Pt)) <= KINDA_SMALL_NUMBER);
				}
			}
			CullDeepPoints(Points, TransformIndex);
			TransformIndex++;
		}
		for (Chaos::FLevelSet* LevelSet : LevelSets)
		{
			TArray<Chaos::FVec3>& Points = CollisionPoints[TransformIndex];
			if (!Points.Num())
			{
				const Chaos::FAABB3& BBox = LevelSet->BoundingBox();
				Chaos::FSphere Sphere(BBox.Center(), BBox.Extents().Size() / 2);
				Points = Sphere.ComputeSamplePoints(ParticlesPerUnitArea, MinParticles, MaxParticles);
				Chaos::FVec3 Normal;
				for (Chaos::FVec3 &Pt : Points)
				{
					const Chaos::FReal Phi = LevelSet->PhiWithNormal(Pt, Normal);
					Pt += Normal * -Phi;
					//check(FMath::Abs(LevelSet->SignedDistance(Pt)) <= KINDA_SMALL_NUMBER);
				}
			}
			CullDeepPoints(Points, TransformIndex);
			TransformIndex++;
		}

		if (Num > 1)
		{
			int32 NumPoints = 0;
			for (const auto& PtArray : CollisionPoints)
				NumPoints += PtArray.Num();
			ContiguousCollisionPoints.Reserve(NumPoints);
		}
		for (TransformIndex = 0; TransformIndex < Num; TransformIndex++)
		{
			TArray<Chaos::FVec3> &PtArray = CollisionPoints[TransformIndex];
			const FTransform& Xf = Transforms[TransformIndex];
			if(!Xf.Equals(FTransform::Identity))
			{
				for (Chaos::FVec3& Pt : PtArray)
					Pt = Xf.TransformPosition(Pt);
			}
			if (Num == 1)
			{
				// Free memory we're not going to use
				ContiguousCollisionPoints.Empty();
				return &PtArray;
			}
			ContiguousCollisionPoints.Append(PtArray);	
		}

		// Free memory we're not going to use
		CollisionPoints.Empty();
		return &ContiguousCollisionPoints;
	}

	TArray<Chaos::TVec3<int32>> BuildSampleTopology() const
	{
		int32 NumTris = 0;
		for (const TArray<Chaos::TVec3<int32>>& Tris : CollisionTriangles)
		{
			NumTris += Tris.Num();
		}

		TArray<Chaos::TVec3<int32>> AllTriangles;
		AllTriangles.Reserve(NumTris);
		int32 Offset = 0;
		for (int32 Index=0; Index < CollisionTriangles.Num(); Index++)
		{
			for (const Chaos::TVec3<int32>& Tri : CollisionTriangles[Index])
			{
				AllTriangles.Add(Tri + Offset);
			}
			Offset += CollisionPoints[Index].Num();
		}

		return AllTriangles;
	}

	/**
	 * Build the implicit object representation of this object.
	 *
	 * Transfers ownership of sub structures to the returned implicit object.
	 */
	Chaos::FImplicitObject* BuildSimImplicitObject()
	{
		// TODO: We make copies of implicit objects owned by this class, so we
		// give the solver memory it can own.  It'd be nice if we could transfer
		// or share the memory this class owns to/with the solver.

		const int32 Num = NumStructures();
		if (Num == 0)
		{
			return nullptr;
		}
		else if (Num == 1)
		{
			if (Transforms[0].Equals(FTransform::Identity))
			{
				return TransferImplicitObj(0);
			}
			else
			{
				// Make a copy and transfer ownership to the transformed implicit.
				Chaos::FImplicitObjectPtr ObjPtr(TransferImplicitObj(0));
				return new Chaos::TImplicitObjectTransformed<Chaos::FReal, 3, true>(
					MoveTemp(ObjPtr),
					Chaos::FRigidTransform3(Transforms[0]));
			}
		}
		else
		{
			// Make copies of the implicits owned by transformed immplicits, and 
			// transfer ownership of the transformed implicits to the implicit union.
			TArray<Chaos::FImplicitObjectPtr> ImplicitObjects;
			ImplicitObjects.Reserve(Num);
			for (int i = 0; i < Num; i++)
			{
				Chaos::FImplicitObjectPtr ObjPtr(TransferImplicitObj(i));

				const FTransform &Xf = Transforms[i];
				if (Xf.Equals(FTransform::Identity))
				{
					// If we ever support animated substructures, we'll need all
					// of them wrapped by a transform.
					ImplicitObjects.Add(MoveTemp(ObjPtr));
				}
				else
				{
					ImplicitObjects.Add(
						Chaos::FImplicitObjectPtr(
							new Chaos::TImplicitObjectTransformed<Chaos::FReal, 3, true>(
								MoveTemp(ObjPtr),
								Chaos::FRigidTransform3(Xf))));
				}
			}
			return new Chaos::FImplicitObjectUnion(MoveTemp(ImplicitObjects));
		}
	}

protected:
	friend class FBoneHierarchy;

	void SetParent(FAnalyticImplicitGroup *InParent)
	{ Parent = InParent; }
	const FAnalyticImplicitGroup* GetParent() const
	{ return Parent; }

	void AddChild(FAnalyticImplicitGroup *Child)
	{ Children.Add(Child); }
	const TArray<FAnalyticImplicitGroup*>& GetChildren() const
	{ return Children; }

	void ClearHierarchy()
	{ Parent = nullptr; Children.Reset(); }

	template<class TImplicitShape>
	void CullDeepPoints(TArray<Chaos::FVec3>& Points, const TImplicitShape& Shape, const FTransform& Xf)
	{
		const Chaos::FAABB3& BBox = Shape.BoundingBox();
		const Chaos::FReal Tolerance = -BBox.Extents().Max() / (Chaos::FReal)100.; // -1/100th the largest dimension
		if (Xf.Equals(FTransform::Identity))
		{
			for (int32 i = Points.Num(); i--; )
			{
				const Chaos::FVec3& LocalPoint = Points[i];
				const Chaos::FReal Phi = Shape.SignedDistance(LocalPoint);
				if (Phi < Tolerance)
				{
					Points.RemoveAt(i);
				}
			}
		}
		else
		{
			const FTransform InvXf = Xf.Inverse();
			for (int32 i = Points.Num(); i--; )
			{
				const Chaos::FVec3 LocalPoint = InvXf.TransformPosition(Points[i]);
				const Chaos::FReal Phi = Shape.SignedDistance(LocalPoint);
				if (Phi < Tolerance)
				{
					Points.RemoveAt(i);
				}
			}
		}
	}

	void CullDeepPoints(TArray<Chaos::FVec3>& Points, const int32 SkipIndex)
	{
		int32 TransformIndex = 0;
		for (Chaos::FSphere* Sphere : Spheres)
		{
			if (TransformIndex != SkipIndex)
			{
				CullDeepPoints(Points, *Sphere, Transforms[TransformIndex]);
			}
			TransformIndex++;
		}
		for (Chaos::TBox<Chaos::FReal, 3>* Box : Boxes)
		{
			if (TransformIndex != SkipIndex)
			{
				CullDeepPoints(Points, *Box, Transforms[TransformIndex]);
			}
			TransformIndex++;
		}
		for (Chaos::FCapsule* Capsule : Capsules)
		{
			if (TransformIndex != SkipIndex)
			{
				CullDeepPoints(Points, *Capsule, Transforms[TransformIndex]);
			}
			TransformIndex++;
		}
		for (Chaos::FTaperedCylinder* TaperedCylinder : TaperedCylinders)
		{
			if (TransformIndex != SkipIndex)
			{
				CullDeepPoints(Points, *TaperedCylinder, Transforms[TransformIndex]);
			}
			TransformIndex++;
		}
		for (Chaos::FConvex* Convex : ConvexHulls)
		{
			if (TransformIndex != SkipIndex)
			{
				CullDeepPoints(Points, *Convex, Transforms[TransformIndex]);
			}
			TransformIndex++;
		}
		for (Chaos::FLevelSet* LevelSet : LevelSets)
		{
			if (TransformIndex != SkipIndex)
			{
				CullDeepPoints(Points, *LevelSet, Transforms[TransformIndex]);
			}
			TransformIndex++;
		}
	}

	Chaos::FImplicitObject* TransferImplicitObj(int32 Idx)
	{
		Chaos::FImplicitObject* Obj = nullptr;
		if (Idx < Spheres.Num())
		{
			Obj = Spheres[Idx]; 
			Spheres[Idx] = nullptr;
			return Obj;
		}
		Idx -= Spheres.Num();

		if (Idx < Boxes.Num())
		{
			Obj = Boxes[Idx];
			Boxes[Idx] = nullptr;
			return Obj;
		}
		Idx -= Boxes.Num();

		if (Idx < Capsules.Num())
		{
			Obj = Capsules[Idx];
			Capsules[Idx] = nullptr;
			return Obj;
		}
		Idx -= Capsules.Num();

		if (Idx < TaperedCylinders.Num())
		{
			Obj = TaperedCylinders[Idx];
			TaperedCylinders[Idx] = nullptr;
			return Obj;
		}
		Idx -= TaperedCylinders.Num();

		if (Idx < ConvexHulls.Num())
		{
			Obj = ConvexHulls[Idx];
			ConvexHulls[Idx] = nullptr;
			return Obj;
		}
		Idx -= ConvexHulls.Num();

		if (Idx < LevelSets.Num())
		{
			Obj = LevelSets[Idx];
			LevelSets[Idx] = nullptr;
			return Obj;
		}

		check(false); // Index out of range!
		return Obj;
	}

protected:
	//friend class USkeletalMeshSimulationComponent;
	friend struct FPhysicsAssetSimulationUtil;

	FName BoneName;
	int32 BoneIndex;
	int32 ParentBoneIndex;

	int32 RigidBodyId;
	EObjectStateTypeEnum RigidBodyState;

	// FkSphereElem and FKTaperedCapsuleElem ends
	TArray<Chaos::FSphere*> Spheres;
	// FKBoxElem
	TArray<Chaos::TBox<Chaos::FReal, 3>*> Boxes;
	// FKSphylElem - Z axis is capsule axis
	TArray<Chaos::FCapsule*> Capsules;
	// FKTaperedCapsuleElem - Z axis is the capsule axis
	TArray<Chaos::FTaperedCylinder*> TaperedCylinders;
	// FKConvexElem
	TArray<Chaos::FConvex*> ConvexHulls;
	// Chaos::TConvex replacement
	TArray<Chaos::FLevelSet*> LevelSets;

	TArray<Chaos::FVec3> ContiguousCollisionPoints;
	TArray<TArray<Chaos::FVec3>> CollisionPoints;
	TArray<TArray<Chaos::TVec3<int32>>> CollisionTriangles;

	TArray<FTransform> Transforms;
	FTransform RefBoneXf;

	FAnalyticImplicitGroup* Parent;
	TArray<FAnalyticImplicitGroup*> Children;
};