CGNICO/UnrealEngine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
c11d382a6f0acddb0bc1e95ab9bc5f8fc3570b55
UnrealEngine/Engine/Source/Programs/HeadlessChaos/Private/HeadlessChaosTestUtility.cpp
Jeffreytsai1004 c11d382a6f ue5-main
2025-05-18 13:04:45 +08:00

748 lines
30 KiB
C++
Raw Blame History

// Copyright Epic Games, Inc. All Rights Reserved.
#include "HeadlessChaosTestUtility.h"
#include "HeadlessChaos.h"
#include "Chaos/Box.h"
#include "Chaos/Convex.h"
#include "Chaos/ErrorReporter.h"
#include "Chaos/Levelset.h"
#include "Chaos/ParticleHandle.h"
#include "Chaos/PBDRigidParticles.h"
#include "Chaos/PBDRigidsEvolutionGBF.h"
#include "Chaos/Plane.h"
#include "Chaos/Sphere.h"
#include "Chaos/Cylinder.h"
#include "Chaos/TaperedCylinder.h"
#include "Chaos/Utilities.h"
namespace ChaosTest {
using namespace Chaos;
int32 AppendAnalyticSphere(FPBDRigidParticles& InParticles, FReal Scale)
{
InParticles.AddParticles(1);
int32 RigidBodyIndex = InParticles.Size() - 1;
InParticles.SetX(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetV(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetR(RigidBodyIndex, FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
InParticles.SetW(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetP(RigidBodyIndex, InParticles.GetX(RigidBodyIndex));
InParticles.SetQ(RigidBodyIndex, InParticles.GetR(RigidBodyIndex));
InParticles.M(RigidBodyIndex) = 1.0;
InParticles.InvM(RigidBodyIndex) = 1.0;
InParticles.I(RigidBodyIndex) = TVec3<FRealSingle>(1, 1, 1);
InParticles.InvI(RigidBodyIndex) = TVec3<FRealSingle>(1, 1, 1);
InParticles.SetGeometry(RigidBodyIndex, MakeImplicitObjectPtr<Chaos::FSphere>(FVec3(0), Scale));
InParticles.SetObjectState(RigidBodyIndex, EObjectStateType::Dynamic);
return RigidBodyIndex;
}
int32 AppendAnalyticBox(FPBDRigidParticles& InParticles, FVec3 Scale)
{
InParticles.AddParticles(1);
int32 RigidBodyIndex = InParticles.Size() - 1;
InParticles.SetX(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetV(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetR(RigidBodyIndex, FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
InParticles.SetW(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetP(RigidBodyIndex, InParticles.GetX(RigidBodyIndex));
InParticles.SetQ(RigidBodyIndex, InParticles.GetR(RigidBodyIndex));
InParticles.M(RigidBodyIndex) = 1.0;
InParticles.InvM(RigidBodyIndex) = 1.0;
InParticles.I(RigidBodyIndex) = TVec3<FRealSingle>(1, 1, 1);
InParticles.InvI(RigidBodyIndex) = TVec3<FRealSingle>(1, 1, 1);
InParticles.SetGeometry(RigidBodyIndex, MakeImplicitObjectPtr<TBox<FReal, 3>>(-Scale / 2.0, Scale / 2.0));
InParticles.SetObjectState(RigidBodyIndex, EObjectStateType::Dynamic);
return RigidBodyIndex;
}
void InitAnalyticBox2(FKinematicGeometryParticleHandle* Particle, FVec3 Scale)
{
Particle->SetX(FVec3(0, 0, 0));
Particle->SetV(FVec3(0, 0, 0));
Particle->SetR(FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
Particle->SetW(FVec3(0, 0, 0));
Particle->SetGeometry(MakeImplicitObjectPtr<TBox<FReal, 3>>(-Scale / 2.0, Scale / 2.0));
FPBDRigidParticleHandle* DynamicParticle = Particle->CastToRigidParticle();
if(DynamicParticle && DynamicParticle->ObjectState() == EObjectStateType::Dynamic)
{
DynamicParticle->SetP(Particle->GetX());
DynamicParticle->SetQ(Particle->GetR());
DynamicParticle->M() = 1.0;
DynamicParticle->InvM() = 1.0;
DynamicParticle->I() = TVec3<FRealSingle>(1, 1, 1);
DynamicParticle->InvI() = TVec3<FRealSingle>(1, 1, 1);
}
}
int32 AppendParticleBox(FPBDRigidParticles& InParticles, FVec3 Scale, TArray<TVec3<int32>>* elements)
{
InParticles.AddParticles(1);
int32 RigidBodyIndex = InParticles.Size() - 1;
InParticles.SetX(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetV(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetR(RigidBodyIndex, FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
InParticles.SetW(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetP(RigidBodyIndex, InParticles.GetX(RigidBodyIndex));
InParticles.SetQ(RigidBodyIndex, InParticles.GetR(RigidBodyIndex));
check(Scale.X == Scale.Y && Scale.X == Scale.Z);
FReal ScaleSq = Scale.X * Scale.X;
InParticles.M(RigidBodyIndex) = 1.0;
InParticles.InvM(RigidBodyIndex) = 1.0;
InParticles.I(RigidBodyIndex) = TVec3<FRealSingle>(ScaleSq / 6.0, ScaleSq / 6.0, ScaleSq / 6.0);
InParticles.InvI(RigidBodyIndex) = TVec3<FRealSingle>(6.0 / ScaleSq, 6.0 / ScaleSq, 6.0 / ScaleSq);
InParticles.SetGeometry(RigidBodyIndex, MakeImplicitObjectPtr<TBox<FReal, 3>>(-Scale / 2.0, Scale / 2.0));
InParticles.SetObjectState(RigidBodyIndex, EObjectStateType::Dynamic);
int32 CollisionIndex = 0;
InParticles.CollisionParticlesInitIfNeeded(RigidBodyIndex);
InParticles.CollisionParticles(RigidBodyIndex)->AddParticles(8);
InParticles.CollisionParticles(RigidBodyIndex)->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, -Scale[1] / 2.0, -Scale[2] / 2.0));
InParticles.CollisionParticles(RigidBodyIndex)->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, -Scale[1] / 2.0, -Scale[2] / 2.0));
InParticles.CollisionParticles(RigidBodyIndex)->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, +Scale[1] / 2.0, -Scale[2] / 2.0));
InParticles.CollisionParticles(RigidBodyIndex)->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, +Scale[1] / 2.0, -Scale[2] / 2.0));
InParticles.CollisionParticles(RigidBodyIndex)->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, -Scale[1] / 2.0, +Scale[2] / 2.0));
InParticles.CollisionParticles(RigidBodyIndex)->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, -Scale[1] / 2.0, +Scale[2] / 2.0));
InParticles.CollisionParticles(RigidBodyIndex)->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, +Scale[1] / 2.0, +Scale[2] / 2.0));
InParticles.CollisionParticles(RigidBodyIndex)->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, +Scale[1] / 2.0, +Scale[2] / 2.0));
if (elements != nullptr)
{
/*
//cw
elements->Add(TVec3<int32>(4, 1, 5)); // Front
elements->Add(TVec3<int32>(1, 4, 0));
elements->Add(TVec3<int32>(7, 2, 6)); // Back
elements->Add(TVec3<int32>(2, 7, 3));
elements->Add(TVec3<int32>(6, 0, 4)); // Right
elements->Add(TVec3<int32>(0, 6, 2));
elements->Add(TVec3<int32>(5, 3, 7)); // Left
elements->Add(TVec3<int32>(3, 5, 1));
elements->Add(TVec3<int32>(6, 5, 7)); // Top
elements->Add(TVec3<int32>(5, 6, 4));
elements->Add(TVec3<int32>(0, 2, 1)); // Front
elements->Add(TVec3<int32>(2, 0, 3));
*/
//ccw
elements->Add(TVec3<int32>(1,4,5)); // Front
elements->Add(TVec3<int32>(4,1,0));
elements->Add(TVec3<int32>(2,7,6)); // Back
elements->Add(TVec3<int32>(7,2,3));
elements->Add(TVec3<int32>(0,6,4)); // Right
elements->Add(TVec3<int32>(6,0,2));
elements->Add(TVec3<int32>(3,5,7)); // Left
elements->Add(TVec3<int32>(5,3,1));
elements->Add(TVec3<int32>(5,6,7)); // Top
elements->Add(TVec3<int32>(6,5,4));
elements->Add(TVec3<int32>(2,0,1)); // Front
elements->Add(TVec3<int32>(0,2,3));
}
return RigidBodyIndex;
}
void InitDynamicParticleBox2(FPBDRigidParticleHandle* Particle, const FVec3& Scale, FReal Margin, TArray<TVector<int32, 3>>* OutElements)
{
Particle->SetX(FVec3(0, 0, 0));
Particle->SetV(FVec3(0, 0, 0));
Particle->SetR(FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
Particle->SetW(FVec3(0, 0, 0));
Particle->SetP(Particle->GetX());
Particle->SetQ(Particle->GetR());
// Assume unit mass - this gets scaled externally by the actual mass
const FVec3 ScaleSq = Scale * Scale;
Particle->M() = 1.0;
Particle->InvM() = 1.0;
Particle->I() = (1.0 / 12.0) * TVec3<FRealSingle>(ScaleSq.Y + ScaleSq.Z, ScaleSq.X + ScaleSq.Z, ScaleSq.X + ScaleSq.Y);
Particle->InvI() = TVec3<FRealSingle>(6.0 / ScaleSq);
Particle->SetGeometry(MakeImplicitObjectPtr<TBox<FReal, 3>>(-Scale / 2.0, Scale / 2.0, Margin));
int32 CollisionIndex = 0;
Particle->CollisionParticlesInitIfNeeded();
Particle->CollisionParticles()->AddParticles(8);
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, -Scale[1] / 2.0, -Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, -Scale[1] / 2.0, -Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, +Scale[1] / 2.0, -Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, +Scale[1] / 2.0, -Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, -Scale[1] / 2.0, +Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, -Scale[1] / 2.0, +Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, +Scale[1] / 2.0, +Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, +Scale[1] / 2.0, +Scale[2] / 2.0));
Particle->SetLocalBounds(Particle->GetGeometry()->BoundingBox());
Particle->UpdateWorldSpaceState(FRigidTransform3::Identity, FVec3(0));
Particle->SetHasBounds(true);
if (OutElements != nullptr)
{
/*
//cw
OutElements->Add(TVec3<int32>(4, 1, 5)); // Front
OutElements->Add(TVec3<int32>(1, 4, 0));
OutElements->Add(TVec3<int32>(7, 2, 6)); // Back
OutElements->Add(TVec3<int32>(2, 7, 3));
OutElements->Add(TVec3<int32>(6, 0, 4)); // Right
OutElements->Add(TVec3<int32>(0, 6, 2));
OutElements->Add(TVec3<int32>(5, 3, 7)); // Left
OutElements->Add(TVec3<int32>(3, 5, 1));
OutElements->Add(TVec3<int32>(6, 5, 7)); // Top
OutElements->Add(TVec3<int32>(5, 6, 4));
OutElements->Add(TVec3<int32>(0, 2, 1)); // Front
OutElements->Add(TVec3<int32>(2, 0, 3));
*/
//ccw
OutElements->Add(TVec3<int32>(1, 4, 5)); // Front
OutElements->Add(TVec3<int32>(4, 1, 0));
OutElements->Add(TVec3<int32>(2, 7, 6)); // Back
OutElements->Add(TVec3<int32>(7, 2, 3));
OutElements->Add(TVec3<int32>(0, 6, 4)); // Right
OutElements->Add(TVec3<int32>(6, 0, 2));
OutElements->Add(TVec3<int32>(3, 5, 7)); // Left
OutElements->Add(TVec3<int32>(5, 3, 1));
OutElements->Add(TVec3<int32>(5, 6, 7)); // Top
OutElements->Add(TVec3<int32>(6, 5, 4));
OutElements->Add(TVec3<int32>(2, 0, 1)); // Front
OutElements->Add(TVec3<int32>(0, 2, 3));
}
::ChaosTest::SetParticleSimDataToCollide({ Particle});
}
void InitDynamicParticleSphere2(FPBDRigidParticleHandle* Particle, const FVec3& Scale, TArray<TVec3<int32>>* OutElements)
{
Particle->SetX(FVec3(0, 0, 0));
Particle->SetV(FVec3(0, 0, 0));
Particle->SetR(FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
Particle->SetW(FVec3(0, 0, 0));
Particle->SetP(Particle->GetX());
Particle->SetQ(Particle->GetR());
check(Scale.X == Scale.Y && Scale.X == Scale.Z);
FReal ScaleSq = Scale.X * Scale.X;
Particle->M() = 1.0;
Particle->InvM() = 1.0;
Particle->I() = TVec3<FRealSingle>(ScaleSq / 6.0);
Particle->InvI() = TVec3<FRealSingle>(6.0 / ScaleSq);
Particle->SetGeometry(MakeImplicitObjectPtr<Chaos::FSphere>(FVec3(0), Scale.X / 2.0));
int32 CollisionIndex = 0;
Particle->CollisionParticlesInitIfNeeded();
Particle->CollisionParticles()->AddParticles(6);
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, 0, 0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, 0, 0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(0, -Scale[1] / 2.0, 0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(0, +Scale[1] / 2.0, 0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(0, 0, -Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(0, 0, +Scale[2] / 2.0));
if (OutElements != nullptr)
{
/*
//cw
OutElements->Add(TVec3<int32>(4, 1, 5)); // Front
OutElements->Add(TVec3<int32>(1, 4, 0));
OutElements->Add(TVec3<int32>(7, 2, 6)); // Back
OutElements->Add(TVec3<int32>(2, 7, 3));
OutElements->Add(TVec3<int32>(6, 0, 4)); // Right
OutElements->Add(TVec3<int32>(0, 6, 2));
OutElements->Add(TVec3<int32>(5, 3, 7)); // Left
OutElements->Add(TVec3<int32>(3, 5, 1));
OutElements->Add(TVec3<int32>(6, 5, 7)); // Top
OutElements->Add(TVec3<int32>(5, 6, 4));
OutElements->Add(TVec3<int32>(0, 2, 1)); // Front
OutElements->Add(TVec3<int32>(2, 0, 3));
*/
//ccw
OutElements->Add(TVec3<int32>(1, 4, 5)); // Front
OutElements->Add(TVec3<int32>(4, 1, 0));
OutElements->Add(TVec3<int32>(2, 7, 6)); // Back
OutElements->Add(TVec3<int32>(7, 2, 3));
OutElements->Add(TVec3<int32>(0, 6, 4)); // Right
OutElements->Add(TVec3<int32>(6, 0, 2));
OutElements->Add(TVec3<int32>(3, 5, 7)); // Left
OutElements->Add(TVec3<int32>(5, 3, 1));
OutElements->Add(TVec3<int32>(5, 6, 7)); // Top
OutElements->Add(TVec3<int32>(6, 5, 4));
OutElements->Add(TVec3<int32>(2, 0, 1)); // Front
OutElements->Add(TVec3<int32>(0, 2, 3));
}
}
void InitDynamicParticleCylinder2(FPBDRigidParticleHandle* Particle, const FVec3& Scale, TArray<TVec3<int32>>* OutElements, bool Tapered)
{
Particle->SetX(FVec3(0, 0, 0));
Particle->SetV(FVec3(0, 0, 0));
Particle->SetR(FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
Particle->SetW(FVec3(0, 0, 0));
Particle->SetP(Particle->GetX());
Particle->SetQ(Particle->GetR());
check(Scale.X == Scale.Y && Scale.X == Scale.Z);
FReal ScaleSq = Scale.X * Scale.X;
Particle->M() = 1.0;
Particle->InvM() = 1.0;
Particle->I() = TVec3<FRealSingle>(ScaleSq / 6.0);
Particle->InvI() = TVec3<FRealSingle>(6.0 / ScaleSq);
if (Tapered)
{
Particle->SetGeometry(MakeImplicitObjectPtr<FTaperedCylinder>(FVec3(0, 0, Scale.X / 2.0), FVec3(0, 0, -Scale.X / 2.0), Scale.X / 2.0, Scale.X / 2.0));
}
else
{
Particle->SetGeometry(MakeImplicitObjectPtr<FCylinder>(FVec3(0, 0, Scale.X / 2.0), FVec3(0, 0, -Scale.X / 2.0), Scale.X / 2.0));
}
int32 CollisionIndex = 0;
Particle->CollisionParticlesInitIfNeeded();
Particle->CollisionParticles()->AddParticles(8);
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, 0, +Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(-Scale[0] / 2.0, 0, -Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, 0, +Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(+Scale[0] / 2.0, 0, -Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(0, -Scale[1] / 2.0, +Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(0, -Scale[1] / 2.0, -Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(0, +Scale[1] / 2.0, +Scale[2] / 2.0));
Particle->CollisionParticles()->SetX(CollisionIndex++, FVec3(0, +Scale[1] / 2.0, -Scale[2] / 2.0));
if (OutElements != nullptr)
{
/*
//cw
OutElements->Add(TVec3<int32>(4, 1, 5)); // Front
OutElements->Add(TVec3<int32>(1, 4, 0));
OutElements->Add(TVec3<int32>(7, 2, 6)); // Back
OutElements->Add(TVec3<int32>(2, 7, 3));
OutElements->Add(TVec3<int32>(6, 0, 4)); // Right
OutElements->Add(TVec3<int32>(0, 6, 2));
OutElements->Add(TVec3<int32>(5, 3, 7)); // Left
OutElements->Add(TVec3<int32>(3, 5, 1));
OutElements->Add(TVec3<int32>(6, 5, 7)); // Top
OutElements->Add(TVec3<int32>(5, 6, 4));
OutElements->Add(TVec3<int32>(0, 2, 1)); // Front
OutElements->Add(TVec3<int32>(2, 0, 3));
*/
//ccw
OutElements->Add(TVec3<int32>(1, 4, 5)); // Front
OutElements->Add(TVec3<int32>(4, 1, 0));
OutElements->Add(TVec3<int32>(2, 7, 6)); // Back
OutElements->Add(TVec3<int32>(7, 2, 3));
OutElements->Add(TVec3<int32>(0, 6, 4)); // Right
OutElements->Add(TVec3<int32>(6, 0, 2));
OutElements->Add(TVec3<int32>(3, 5, 7)); // Left
OutElements->Add(TVec3<int32>(5, 3, 1));
OutElements->Add(TVec3<int32>(5, 6, 7)); // Top
OutElements->Add(TVec3<int32>(6, 5, 4));
OutElements->Add(TVec3<int32>(2, 0, 1)); // Front
OutElements->Add(TVec3<int32>(0, 2, 3));
}
}
FPBDRigidParticleHandle* AppendDynamicParticleBox(FPBDRigidsSOAs& SOAs, const FVec3& Scale, TArray<TVec3<int32>>* OutElements)
{
TArray<FPBDRigidParticleHandle*> Particles = SOAs.CreateDynamicParticles(1);
InitDynamicParticleBox2(Particles[0], Scale, 0.0f, OutElements);
return Particles[0];
}
// Create a particle with box collision of specified size and margin (size includes margin)
FPBDRigidParticleHandle* AppendDynamicParticleBoxMargin(FPBDRigidsSOAs& SOAs, const FVec3& Scale, FReal Margin, TArray<TVec3<int32>>* OutElements)
{
TArray<FPBDRigidParticleHandle*> Particles = SOAs.CreateDynamicParticles(1);
InitDynamicParticleBox2(Particles[0], Scale, Margin, OutElements);
return Particles[0];
}
FPBDRigidParticleHandle* AppendDynamicParticleSphere(FPBDRigidsSOAs& SOAs, const FVec3& Scale, TArray<TVec3<int32>>* OutElements)
{
TArray<FPBDRigidParticleHandle*> Particles = SOAs.CreateDynamicParticles(1);
InitDynamicParticleSphere2(Particles[0], Scale, OutElements);
return Particles[0];
}
FPBDRigidParticleHandle* AppendDynamicParticleCylinder(FPBDRigidsSOAs& SOAs, const FVec3& Scale, TArray<TVec3<int32>>* OutElements)
{
TArray<FPBDRigidParticleHandle*> Particles = SOAs.CreateDynamicParticles(1);
InitDynamicParticleCylinder2(Particles[0], Scale, OutElements, false);
return Particles[0];
}
FPBDRigidParticleHandle* AppendDynamicParticleTaperedCylinder(FPBDRigidsSOAs& SOAs, const FVec3& Scale, TArray<TVec3<int32>>* OutElements)
{
TArray<FPBDRigidParticleHandle*> Particles = SOAs.CreateDynamicParticles(1);
InitDynamicParticleCylinder2(Particles[0], Scale, OutElements, true);
return Particles[0];
}
FPBDRigidParticleHandle* AppendClusteredParticleBox(FPBDRigidsSOAs& SOAs, const FVec3& Scale, TArray<TVec3<int32>>* OutElements)
{
auto Particles = SOAs.CreateClusteredParticles(1);
InitDynamicParticleBox2(Particles[0], Scale, 0.0f, OutElements);
return Particles[0];
}
void InitStaticParticleBox(FGeometryParticleHandle* Particle, const FVec3& Scale, TArray<TVec3<int32>>* OutElements)
{
Particle->SetX(FVec3(0, 0, 0));
Particle->SetR(FRotation3::FromIdentity());
FReal ScaleSq = Scale.X * Scale.X;
Particle->SetGeometry(MakeImplicitObjectPtr<TBox<FReal, 3>>(-Scale / 2.0, Scale / 2.0));
// This is needed for calculating contacts (Bounds are bigger than they need to be, even allowing for rotation)
Particle->SetLocalBounds(FAABB3(FVec3(-Scale[0]), FVec3(Scale[0])));
Particle->UpdateWorldSpaceState(FRigidTransform3::Identity, FVec3(0));
Particle->SetHasBounds(true);
if (OutElements != nullptr)
{
/*
//cw
OutElements->Add(TVec3<int32>(4, 1, 5)); // Front
OutElements->Add(TVec3<int32>(1, 4, 0));
OutElements->Add(TVec3<int32>(7, 2, 6)); // Back
OutElements->Add(TVec3<int32>(2, 7, 3));
OutElements->Add(TVec3<int32>(6, 0, 4)); // Right
OutElements->Add(TVec3<int32>(0, 6, 2));
OutElements->Add(TVec3<int32>(5, 3, 7)); // Left
OutElements->Add(TVec3<int32>(3, 5, 1));
OutElements->Add(TVec3<int32>(6, 5, 7)); // Top
OutElements->Add(TVec3<int32>(5, 6, 4));
OutElements->Add(TVec3<int32>(0, 2, 1)); // Front
OutElements->Add(TVec3<int32>(2, 0, 3));
*/
//ccw
OutElements->Add(TVec3<int32>(1, 4, 5)); // Front
OutElements->Add(TVec3<int32>(4, 1, 0));
OutElements->Add(TVec3<int32>(2, 7, 6)); // Back
OutElements->Add(TVec3<int32>(7, 2, 3));
OutElements->Add(TVec3<int32>(0, 6, 4)); // Right
OutElements->Add(TVec3<int32>(6, 0, 2));
OutElements->Add(TVec3<int32>(3, 5, 7)); // Left
OutElements->Add(TVec3<int32>(5, 3, 1));
OutElements->Add(TVec3<int32>(5, 6, 7)); // Top
OutElements->Add(TVec3<int32>(6, 5, 4));
OutElements->Add(TVec3<int32>(2, 0, 1)); // Front
OutElements->Add(TVec3<int32>(0, 2, 3));
}
::ChaosTest::SetParticleSimDataToCollide({ Particle });
}
FGeometryParticleHandle* AppendStaticParticleBox(FPBDRigidsSOAs& SOAs, const FVec3& Scale, TArray<TVec3<int32>>* OutElements)
{
TArray<FGeometryParticleHandle*> Particles = SOAs.CreateStaticParticles(1);
InitStaticParticleBox(Particles[0], Scale, OutElements);
return Particles[0];
}
int AppendStaticAnalyticFloor(FPBDRigidParticles& InParticles)
{
InParticles.AddParticles(1);
int32 RigidBodyIndex = InParticles.Size() - 1;
InParticles.SetX(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetV(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.SetR(RigidBodyIndex, FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
InParticles.SetW(RigidBodyIndex, FVec3(0, 0, 0));
InParticles.M(RigidBodyIndex) = 1.0;
InParticles.InvM(RigidBodyIndex) = 0.0;
InParticles.I(RigidBodyIndex) = TVec3<FRealSingle>(1);
InParticles.InvI(RigidBodyIndex) = TVec3<FRealSingle>(0);
InParticles.SetGeometry(RigidBodyIndex, MakeImplicitObjectPtr<TPlane<FReal, 3>>(FVec3(0, 0, 0), FVec3(0, 0, 1)));
InParticles.SetObjectState(RigidBodyIndex, EObjectStateType::Kinematic);
InParticles.SetP(RigidBodyIndex, InParticles.GetX(RigidBodyIndex));
InParticles.SetQ(RigidBodyIndex, InParticles.GetR(RigidBodyIndex));
return RigidBodyIndex;
}
FKinematicGeometryParticleHandle* AppendStaticAnalyticFloor(FPBDRigidsSOAs& SOAs)
{
TArray<FKinematicGeometryParticleHandle*> Particles = SOAs.CreateKinematicParticles(1);
FKinematicGeometryParticleHandle* Particle = Particles[0];
Particle->SetX(FVec3(0, 0, 0));
Particle->SetV(FVec3(0, 0, 0));
Particle->SetR(FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
Particle->SetW(FVec3(0, 0, 0));
Particle->SetGeometry(MakeImplicitObjectPtr<TPlane<FReal, 3>>(FVec3(0, 0, 0), FVec3(0, 0, 1)));
::ChaosTest::SetParticleSimDataToCollide({ Particle });
return Particle;
}
FKinematicGeometryParticleHandle* AppendStaticConvexFloor(FPBDRigidsSOAs& SOAs)
{
TArray<FKinematicGeometryParticleHandle*> Particles = SOAs.CreateKinematicParticles(1);
FKinematicGeometryParticleHandle* Particle = Particles[0];
Particle->SetX(FVec3(0, 0, 0));
Particle->SetV(FVec3(0, 0, 0));
Particle->SetR(FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
Particle->SetW(FVec3(0, 0, 0));
TArray<Chaos::FConvex::FVec3Type> Cube;
Cube.SetNum(9);
Cube[0] = { -1000, -1000, -20 };
Cube[1] = { -1000, -1000, 0 };
Cube[2] = { -1000, 1000, -20 };
Cube[3] = { -1000, 1000, 0 };
Cube[4] = { 1000, -1000, -20 };
Cube[5] = { 1000, -1000, 0 };
Cube[6] = { 1000, 1000, -20 };
Cube[7] = { 1000, 1000, 0 };
Cube[8] = { 0, 0, 0 };
Particle->SetGeometry(MakeImplicitObjectPtr<FConvex>(Cube, 0.0f));
::ChaosTest::SetParticleSimDataToCollide({ Particle });
return Particle;
}
/**/
FLevelSet ConstructLevelset(FParticles & SurfaceParticles, TArray<TVec3<int32>> & Elements)
{
// build Particles and bounds
Chaos::FAABB3 BoundingBox(FVec3(0), FVec3(0));
for (int32 CollisionParticleIndex = 0; CollisionParticleIndex < (int32)SurfaceParticles.Size(); CollisionParticleIndex++)
{
BoundingBox.GrowToInclude(SurfaceParticles.GetX(CollisionParticleIndex));
}
// build cell domain
int32 MaxAxisSize = 10;
int MaxAxis = BoundingBox.LargestAxis();
FVec3 Extents = BoundingBox.Extents();
Chaos::TVec3<int32> Counts(MaxAxisSize * Extents[0] / Extents[MaxAxis], MaxAxisSize * Extents[1] / Extents[MaxAxis], MaxAxisSize * Extents[2] / Extents[MaxAxis]);
Counts[0] = Counts[0] < 1 ? 1 : Counts[0];
Counts[1] = Counts[1] < 1 ? 1 : Counts[1];
Counts[2] = Counts[2] < 1 ? 1 : Counts[2];
TUniformGrid<FReal, 3> Grid(BoundingBox.Min(), BoundingBox.Max(), Counts, 1);
FTriangleMesh CollisionMesh(MoveTemp(Elements));
FErrorReporter ErrorReporter;
return FLevelSet(ErrorReporter, Grid, SurfaceParticles, CollisionMesh);
}
/**/
void AppendDynamicParticleConvexBox(FPBDRigidParticleHandle& InParticles, const FVec3& Scale, FReal Margin)
{
FConvex::FVec3Type ScaleFloat{ FRealSingle(Scale.X), FRealSingle(Scale.Y), FRealSingle(Scale.Z) };
TArray<FConvex::FVec3Type> Cube;
Cube.SetNum(9);
Cube[0] = FConvex::FVec3Type{ -1, -1, -1 } * ScaleFloat;
Cube[1] = FConvex::FVec3Type{ -1, -1, 1 } * ScaleFloat;
Cube[2] = FConvex::FVec3Type{ -1, 1, -1 } * ScaleFloat;
Cube[3] = FConvex::FVec3Type{ -1, 1, 1 } * ScaleFloat;
Cube[4] = FConvex::FVec3Type{ 1, -1, -1 } * ScaleFloat;
Cube[5] = FConvex::FVec3Type{ 1, -1, 1 } * ScaleFloat;
Cube[6] = FConvex::FVec3Type{ 1, 1, -1 } * ScaleFloat;
Cube[7] = FConvex::FVec3Type{ 1, 1, 1 } * ScaleFloat;
Cube[8] = FConvex::FVec3Type{ 0, 0, 0 };
InParticles.SetX(FVec3(0, 0, 0));
InParticles.SetV(FVec3(0, 0, 0));
InParticles.SetR(FRotation3::MakeFromEuler(FVec3(0, 0, 0)).GetNormalized());
InParticles.SetW(FVec3(0, 0, 0));
InParticles.SetP(InParticles.GetX());
InParticles.SetQ(InParticles.GetR());
// TODO: Change this error prone API to set bounds more automatically. This is easy to forget
InParticles.SetLocalBounds(FAABB3(Cube[0], Cube[7]));
InParticles.UpdateWorldSpaceState(FRigidTransform3::Identity, FVec3(0));
InParticles.SetHasBounds(true);
InParticles.M() = 1.0;
InParticles.InvM() = 1.0;
InParticles.I() = TVec3<FRealSingle>(1);
InParticles.InvI() = TVec3<FRealSingle>(1);
InParticles.SetGeometry(MakeImplicitObjectPtr<FConvex>(Cube, Margin));
InParticles.SetObjectStateLowLevel(EObjectStateType::Dynamic);
::ChaosTest::SetParticleSimDataToCollide({ &InParticles });
//for (const TUniquePtr<Chaos::FPerShapeData>& Shape : InParticles.ShapesArray())
//{
// Shape->SimData.Word3 = 1;
// Shape->SimData.Word1 = 1;
//}
}
FPBDRigidParticleHandle* AppendDynamicParticleConvexBox(FPBDRigidsSOAs& SOAs, const FVec3& Scale)
{
TArray<FPBDRigidParticleHandle*> Particles = SOAs.CreateDynamicParticles(1);
AppendDynamicParticleConvexBox(*Particles[0], Scale, 0.0f);
return Particles[0];
}
FPBDRigidParticleHandle* AppendDynamicParticleConvexBoxMargin(FPBDRigidsSOAs& SOAs, const FVec3& Scale, FReal Margin)
{
TArray<FPBDRigidParticleHandle*> Particles = SOAs.CreateDynamicParticles(1);
AppendDynamicParticleConvexBox(*Particles[0], Scale, Margin);
return Particles[0];
}
/**/
FVec3 ObjectSpacePoint(FPBDRigidParticles& InParticles, const int32 Index, const FVec3& WorldSpacePoint)
{
FRigidTransform3 LocalToWorld(InParticles.GetX(Index), InParticles.GetR(Index));
return LocalToWorld.InverseTransformPosition(WorldSpacePoint);
}
FVec3 ObjectSpacePoint(FGeometryParticleHandle& Particle, const FVec3& WorldSpacePoint)
{
FRigidTransform3 LocalToWorld(Particle.GetX(), Particle.GetR());
return LocalToWorld.InverseTransformPosition(WorldSpacePoint);
}
/**/
FReal PhiWithNormal(FPBDRigidParticles& InParticles, const int32 Index, const FVec3& WorldSpacePoint, FVec3& Normal)
{
FRigidTransform3(InParticles.GetX(Index), InParticles.GetR(Index));
FVec3 BodySpacePoint = ObjectSpacePoint(InParticles, Index, WorldSpacePoint);
FReal LocalPhi = InParticles.GetGeometry(Index)->PhiWithNormal(BodySpacePoint, Normal);
Normal = FRigidTransform3(InParticles.GetX(Index), InParticles.GetR(Index)).TransformVector(Normal);
return LocalPhi;
}
/**/
FReal SignedDistance(FPBDRigidParticles& InParticles, const int32 Index, const FVec3& WorldSpacePoint)
{
FVec3 Normal;
return PhiWithNormal(InParticles, Index, WorldSpacePoint, Normal);
}
/**/
FReal PhiWithNormal(FGeometryParticleHandle& Particle, const FVec3& WorldSpacePoint, FVec3& Normal)
{
FRigidTransform3(Particle.GetX(), Particle.GetR());
FVec3 BodySpacePoint = ObjectSpacePoint(Particle, WorldSpacePoint);
FReal LocalPhi = Particle.GetGeometry()->PhiWithNormal(BodySpacePoint, Normal);
Normal = FRigidTransform3(Particle.GetX(), Particle.GetR()).TransformVector(Normal);
return LocalPhi;
}
/**/
FReal SignedDistance(FGeometryParticleHandle& Particle, const FVec3& WorldSpacePoint)
{
FVec3 Normal;
return PhiWithNormal(Particle, WorldSpacePoint, Normal);
}
/**/
FVec3 RandAxis()
{
for (int32 It = 0; It < 1000; ++It)
{
FVec3 Point = FVec3(FMath::RandRange(-1.0f, 1.0f), FMath::RandRange(-1.0f, 1.0f), FMath::RandRange(-1.0f, 1.0f));
if (Point.Size() > KINDA_SMALL_NUMBER)
{
return FVec3(Point.GetSafeNormal());
}
}
return FVec3(FVector::UpVector);
}
void SetParticleSimDataToCollide(TArray< Chaos::FGeometryParticle* > ParticleArray)
{
for (Chaos::FGeometryParticle* Particle : ParticleArray)
{
for (const TUniquePtr<Chaos::FPerShapeData>& Shape :Particle->ShapesArray())
{
Shape->ModifySimData([](auto& SimData)
{
SimData.Word3 = 1;
SimData.Word1 = 1;
});
}
}
}
void SetParticleSimDataToCollide(TArray< Chaos::FGeometryParticleHandle* > ParticleArray)
{
for (Chaos::FGeometryParticleHandle* Particle : ParticleArray)
{
for (const TUniquePtr<Chaos::FPerShapeData>& Shape : Particle->ShapesArray())
{
Shape->ModifySimData([](auto& SimData)
{
SimData.Word3 = 1;
SimData.Word1 = 1;
});
}
}
}
TArray<FConvex::FVec3Type> MakeBoxVerts(const FConvex::FVec3Type& Center, const FConvex::FVec3Type& HalfSize)
{
return
{
Center + FConvex::FVec3Type(-HalfSize.X, -HalfSize.Y, -HalfSize.Z),
Center + FConvex::FVec3Type(-HalfSize.X, HalfSize.Y, -HalfSize.Z),
Center + FConvex::FVec3Type(HalfSize.X, HalfSize.Y, -HalfSize.Z),
Center + FConvex::FVec3Type(HalfSize.X, -HalfSize.Y, -HalfSize.Z),
Center + FConvex::FVec3Type(-HalfSize.X, -HalfSize.Y, HalfSize.Z),
Center + FConvex::FVec3Type(-HalfSize.X, HalfSize.Y, HalfSize.Z),
Center + FConvex::FVec3Type(HalfSize.X, HalfSize.Y, HalfSize.Z),
Center + FConvex::FVec3Type(HalfSize.X, -HalfSize.Y, HalfSize.Z),
};
}
FImplicitConvex3 CreateConvexBox(const FConvex::FVec3Type& BoxSize, const FReal Margin)
{
const FConvex::FVec3Type HalfSize = 0.5f * BoxSize;
return FImplicitConvex3(MakeBoxVerts(FConvex::FVec3Type(0), HalfSize), Margin);
}
FImplicitConvex3 CreateConvexBox(const FConvex::FVec3Type& BoxMin, const FConvex::FVec3Type& BoxMax, const FReal Margin)
{
const FConvex::FVec3Type Center = 0.5f * (BoxMin + BoxMax);
const FConvex::FVec3Type HalfSize = 0.5f * (BoxMax - BoxMin);
return FImplicitConvex3(MakeBoxVerts(Center, HalfSize), Margin);
}
TImplicitObjectInstanced<FImplicitConvex3> CreateInstancedConvexBox(const FConvex::FVec3Type& BoxSize, const FReal Margin)
{
const FConvex::FVec3Type HalfSize = 0.5f * BoxSize;
FConvexPtr BoxConvex( new FImplicitConvex3(MakeBoxVerts(FConvex::FVec3Type(0), HalfSize), 0.0f));
return TImplicitObjectInstanced<FImplicitConvex3>(BoxConvex, Margin);
}
TImplicitObjectScaled<FImplicitConvex3> CreateScaledConvexBox(const FConvex::FVec3Type& BoxSize, const FVec3 BoxScale, const FReal Margin)
{
const FConvex::FVec3Type HalfSize = 0.5f * BoxSize;
FConvexPtr BoxConvex( new FImplicitConvex3(MakeBoxVerts(FConvex::FVec3Type(0), HalfSize), 0.0f));
return TImplicitObjectScaled<FImplicitConvex3>(BoxConvex, BoxScale, Margin);
}
}
Reference in New Issue View Git Blame Copy Permalink