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

765 lines
28 KiB
C++
Raw Blame History

// Copyright Epic Games, Inc. All Rights Reserved.
#include "HeadlessChaosTestCollisions.h"
#include "HeadlessChaos.h"
#include "HeadlessChaosCollisionConstraints.h"
#include "Chaos/GJK.h"
#include "Chaos/Pair.h"
#include "Chaos/PBDRigidsEvolution.h"
#include "Chaos/PBDRigidParticles.h"
#include "Chaos/CollisionResolutionTypes.h"
#include "Chaos/CollisionOneShotManifoldsMiscShapes.h"
#include "Chaos/Sphere.h"
#include "Chaos/Utilities.h"
#include "Modules/ModuleManager.h"
#define SMALL_THRESHOLD 1e-4
#define RESET_PQ(Particle) Particle->SetP(Particle->GetX()); Particle->SetQ(Particle->GetR());
#define INVARIANT_XR_START(Particle) FVec3 InvariantPreX_##Particle = Particle->GetX(); FRotation3 InvariantPreR_##Particle = Particle->GetR()
#define INVARIANT_XR_END(Particle) EXPECT_TRUE(InvariantPreX_##Particle.Equals(Particle->GetX())); EXPECT_TRUE(InvariantPreR_##Particle.Equals(Particle->GetR()))
#define INVARIANT_VW_START(Particle) FVec3 InvariantPreV_##Particle = Particle->GetV(); FVec3 InvariantPreW_##Particle = Particle->GetW()
#define INVARIANT_VW_END(Particle) EXPECT_TRUE(InvariantPreV_##Particle.Equals(Particle->GetV())); EXPECT_TRUE(InvariantPreW_##Particle.Equals(Particle->GetW()))
namespace ChaosTest {
using namespace Chaos;
DEFINE_LOG_CATEGORY_STATIC(LogHChaosCollisions, Verbose, All);
void LevelsetConstraint()
{
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)0;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto Box1 = AppendDynamicParticleBox(Particles);
Box1->SetX(FVec3(1.f));
Box1->SetR(FRotation3(FQuat::Identity));
Box1->SetP(Box1->GetX());
Box1->SetQ(Box1->GetR());
Box1->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
Box1->UpdateWorldSpaceState(FRigidTransform3(Box1->GetX(), Box1->GetR()), FVec3(0));
auto Box2 = AppendDynamicParticleBox(Particles);
Box2->SetX(FVec3(0.5f, 0.5f, 1.9f));
Box2->SetR(FRotation3(FQuat::Identity));
Box2->SetP(Box2->GetX());
Box2->SetQ(Box2->GetR());
Box2->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
Box2->UpdateWorldSpaceState(FRigidTransform3(Box2->GetX(), Box2->GetR()), FVec3(0));
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 1, 1);
Collisions.ComputeConstraints(0.f);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FPBDCollisionConstraint& Constraint = Collisions.GetConstraint(0);
if (auto PBDRigid = Constraint.GetParticle0()->CastToRigidParticle())
{
//Question: non dynamics don't have collision particles, seems wrong if the levelset is dynamic and the static is something like a box
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
Collisions.UpdateLevelsetConstraint(Constraint);
EXPECT_EQ(Constraint.GetParticle0(), Box2);
EXPECT_EQ(Constraint.GetParticle1(), Box1);
EXPECT_TRUE(Constraint.CalculateWorldContactNormal().operator==(FVec3(0, 0, 1)));
// The contact point is the average of the surface contact points, so it should be half of Phi inside particle0
EXPECT_NEAR(Constraint.GetPhi(), -0.1f, KINDA_SMALL_NUMBER);
EXPECT_NEAR(Constraint.CalculateWorldContactLocation().Z, 1.45f, KINDA_SMALL_NUMBER);
}
void LevelsetConstraintGJK()
{
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)0;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto Box1 = AppendDynamicParticleConvexBox(Particles, FVec3(1.f) );
Box1->SetX(FVec3(0.f));
Box1->SetR(FRotation3(FQuat::Identity));
Box1->SetP(Box1->GetX());
Box1->SetQ(Box1->GetR());
Box1->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
Box1->UpdateWorldSpaceState(FRigidTransform3(Box1->GetX(), Box1->GetR()), FVec3(0));
auto Box2 = AppendDynamicParticleBox(Particles, FVec3(1.f) );
Box2->SetX(FVec3(1.25f, 0.f, 0.f));
Box2->SetR(FRotation3(FQuat::Identity));
Box2->SetP(Box2->GetX());
Box2->SetQ(Box2->GetR());
Box2->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
Box2->UpdateWorldSpaceState(FRigidTransform3(Box2->GetX(), Box2->GetR()), FVec3(0));
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 1, 1);
Collisions.ComputeConstraints(0.f);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FPBDCollisionConstraint& Constraint = Collisions.GetConstraint(0);
Collisions.UpdateLevelsetConstraint(Constraint);
EXPECT_EQ(Constraint.GetParticle0(), Box2);
EXPECT_EQ(Constraint.GetParticle1(), Box1);
EXPECT_TRUE(Constraint.CalculateWorldContactNormal().operator==(FVec3(0, 0, 1)));
// The contact point is the average of the surface contact points, so it should be half of Phi inside particle0
Chaos::FReal Distance = ChaosTest::SignedDistance(*Constraint.GetParticle0(), Constraint.CalculateWorldContactLocation());
EXPECT_NEAR(Distance, 0.5f * Constraint.GetPhi(), KINDA_SMALL_NUMBER);
}
void CollisionBoxPlane()
{
// test a box and plane in a colliding state
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)1;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
const FReal Dt = FReal(1) / FReal(24.);
auto Floor = AppendStaticAnalyticFloor(Particles);
auto Box = AppendDynamicParticleBox(Particles);
Box->SetP(FVec3(0, 1, 0));
Box->SetQ(FRotation3(FQuat::Identity));
Box->SetV(FVec3(0, 0, -1));
Box->SetPreV(Box->GetV());
Box->SetX(Box->GetP() - Box->GetV() * Dt);
Box->SetR(Box->GetQ());
Box->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
Box->UpdateWorldSpaceState(FRigidTransform3(Box->GetP(), Box->GetQ()), FVec3(0));
const FReal InitialBoxZ = Box->GetX().Z;
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FPBDCollisionConstraint& Constraint = Collisions.GetConstraint(0);
EXPECT_TRUE(Constraint.GetParticle0() != nullptr);
EXPECT_TRUE(Constraint.GetParticle1() != nullptr);
if (auto PBDRigid = Constraint.GetParticle0()->CastToRigidParticle())
{
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
Collisions.GatherInput(Dt);
Collisions.Update(Constraint);
EXPECT_EQ(Constraint.GetParticle0(), Box);
EXPECT_EQ(Constraint.GetParticle1(), Floor);
EXPECT_TRUE(Constraint.CalculateWorldContactNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(Constraint.GetPhi() - FReal(-0.5) ) < SMALL_THRESHOLD );
{
INVARIANT_XR_START(Box);
const int32 NumIts = 8;
Collisions.Apply(Dt, NumIts);
INVARIANT_XR_END(Box);
}
{
RESET_PQ(Box);
{
INVARIANT_XR_START(Box);
INVARIANT_VW_START(Box);
const int32 NumIts = 1;
Collisions.ApplyPushOut(Dt, NumIts);
INVARIANT_XR_END(Box);
INVARIANT_VW_END(Box);
}
}
Collisions.ScatterOutput(Dt);
// Box will not move because the default depentration velocity is zero
const FReal ExpectedBoxZ = InitialBoxZ + Collisions.CollisionConstraints.GetSolverSettings().DepenetrationVelocity * Dt;
EXPECT_NEAR(Box->GetP().Z, ExpectedBoxZ, 1.e-2f);
// Velocity is below the restitution threshold, so expecting 0 velocity despite the fact that restitution is 1
EXPECT_TRUE(Box->GetV().Equals(FVec3(0)));
EXPECT_TRUE(Box->GetW().Equals(FVec3(0)));
}
void CollisionConvexConvex()
{
// test a box and plane in a colliding state
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)0;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto Floor = AppendStaticConvexFloor(Particles);
auto Box = AppendDynamicParticleConvexBox( Particles, FVec3(50) );
Box->SetX(FVec3(0, 0, 49));
Box->SetR(FRotation3(FQuat::Identity));
Box->SetV(FVec3(0, 0, -1));
Box->SetPreV(Box->GetV());
Box->SetP(Box->GetX());
Box->SetQ(Box->GetR());
Box->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
Box->UpdateWorldSpaceState(FRigidTransform3(Box->GetP(), Box->GetQ()), FVec3(0));
const FReal Dt = FReal(1) / FReal(24.);
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FPBDCollisionConstraint* Constraint = &Collisions.GetConstraint(0);
EXPECT_TRUE(Constraint != nullptr);
Collisions.GatherInput(Dt);
Collisions.Update(*Constraint);
EXPECT_EQ(Constraint->GetParticle0(), Box);
EXPECT_EQ(Constraint->GetParticle1(), Floor);
EXPECT_TRUE(Constraint->CalculateWorldContactNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(Constraint->GetPhi() - FReal(-1.0)) < SMALL_THRESHOLD);
{
INVARIANT_XR_START(Box);
Collisions.Apply(Dt, 1);
INVARIANT_XR_END(Box);
}
{
RESET_PQ(Box);
{
INVARIANT_XR_START(Box);
INVARIANT_VW_START(Box);
const int32 NumIts = 10;
Collisions.ApplyPushOut(Dt, NumIts);
INVARIANT_XR_END(Box);
INVARIANT_VW_END(Box);
}
}
Collisions.ScatterOutput(Dt);
//EXPECT_TRUE(Box->P().Equals(FVector(0.f, 0.f, 50.f)));
//EXPECT_TRUE(Box->Q().Equals(FQuat::Identity));
}
void CollisionBoxPlaneZeroResitution()
{
// test a box and plane in a colliding state
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)0;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
const FReal Dt = FReal(1) / FReal(24.);
auto Floor = AppendStaticAnalyticFloor(Particles);
auto Box = AppendDynamicParticleBox(Particles);
Box->SetP(FVec3(0, 1, 0));
Box->SetQ(FRotation3(FQuat::Identity));
Box->SetV(FVec3(0, 0, -1));
Box->SetPreV(Box->GetV());
Box->SetX(Box->GetP() - Box->GetV() * Dt);
Box->SetR(Box->GetR());
Box->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
Box->UpdateWorldSpaceState(FRigidTransform3(Box->GetP(), Box->GetQ()), FVec3(0));
FReal InitialBoxZ = Box->GetX().Z;
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
Collisions.GatherInput(Dt);
FPBDCollisionConstraint& Constraint = Collisions.GetConstraint(0);
EXPECT_EQ(Constraint.GetParticle0(), Box);
EXPECT_EQ(Constraint.GetParticle1(), Floor);
EXPECT_TRUE(Constraint.CalculateWorldContactNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(Constraint.GetPhi() - FReal(-0.5)) < SMALL_THRESHOLD);
{
INVARIANT_XR_START(Box);
Collisions.Apply(Dt, 1);
INVARIANT_XR_END(Box);
}
{
RESET_PQ(Box);
{
INVARIANT_XR_START(Box);
INVARIANT_VW_START(Box);
const int32 NumIts = 10;
Collisions.ApplyPushOut(Dt, NumIts);
INVARIANT_XR_END(Box);
INVARIANT_VW_END(Box);
}
}
Collisions.ScatterOutput(Dt);
// 0 restitution so expecting 0 velocity
EXPECT_TRUE(Box->GetV().Equals(FVec3(0)));
EXPECT_TRUE(Box->GetW().Equals(FVec3(0)));
// Box will not move because the default depentration velocity is zero
const FReal ExpectedBoxZ = InitialBoxZ + Collisions.CollisionConstraints.GetSolverSettings().DepenetrationVelocity * Dt;
EXPECT_TRUE(FMath::IsNearlyEqual(Box->GetP().Z, ExpectedBoxZ, 1.e-2));
}
void CollisionBoxPlaneRestitution()
{
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)1;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
const FReal Dt = FReal(1) / FReal(24.);
auto Floor = AppendStaticAnalyticFloor(Particles);
auto Box = AppendDynamicParticleBox(Particles);
Box->SetP(FVec3(0, 0, 0));
Box->SetQ(FRotation3(FQuat::Identity));
Box->SetV(FVec3(0, 0, -100));
Box->SetPreV(Box->GetV());
Box->SetX(Box->GetP() - Box->GetV() * Dt);
Box->SetR(Box->GetQ());
Box->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
Box->UpdateWorldSpaceState(FRigidTransform3(Box->GetP(), Box->GetQ()), FVec3(0));
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FPBDCollisionConstraint& Constraint = Collisions.GetConstraint(0);
if (auto PBDRigid = Constraint.GetParticle0()->CastToRigidParticle())
{
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
Collisions.GatherInput(Dt);
Collisions.UpdateLevelsetConstraint(Constraint);
EXPECT_EQ(Constraint.GetParticle0(), Box);
EXPECT_EQ(Constraint.GetParticle1(), Floor);
EXPECT_TRUE(Constraint.CalculateWorldContactNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(Constraint.GetPhi() - FReal(-0.5)) < SMALL_THRESHOLD);
{
INVARIANT_XR_START(Box);
Collisions.Apply(Dt, 1);
INVARIANT_XR_END(Box);
}
{
RESET_PQ(Box);
{
INVARIANT_XR_START(Box);
const int32 NumIts = 10;
Collisions.ApplyPushOut(Dt, NumIts);
INVARIANT_XR_END(Box);
}
}
Collisions.ScatterOutput(Dt);
// full restitution, so expecting negative velocity
EXPECT_TRUE(Box->GetV().Equals(FVec3(0.f, 0.f, 100.f)));
EXPECT_TRUE(Box->GetW().Equals(FVec3(0)));
// should end up outside the plane
EXPECT_GE(Box->GetP().Z, -Box->GetGeometry()->BoundingBox().Min().Z);
EXPECT_TRUE(Box->GetQ().Equals(FQuat::Identity));
}
// This test will make sure that a dynamic cube colliding with a static floor will have the correct bounce velocity
// for a restitution of 0.5
void CollisionCubeCubeRestitution()
{
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)0.5;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
const FReal Dt = FReal(1) / FReal(24.);
FGeometryParticleHandle* StaticCube = AppendStaticParticleBox(Particles, FVec3(100.0f));
StaticCube->SetX(FVec3(0, 0, -50.0f));
StaticCube->UpdateWorldSpaceState(FRigidTransform3(StaticCube->GetX(), StaticCube->GetR()), FVec3(0));
FPBDRigidParticleHandle* DynamicCube = AppendDynamicParticleBox(Particles, FVec3(100.0f));
DynamicCube->SetX(FVec3(0, 0, 50));
DynamicCube->SetR(FRotation3::FromIdentity());
DynamicCube->SetV(FVec3(0, 0, -100));
DynamicCube->SetPreV(DynamicCube->GetV());
DynamicCube->SetP(DynamicCube->GetX() + DynamicCube->GetV() * Dt);
DynamicCube->SetQ(DynamicCube->GetR());
DynamicCube->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
DynamicCube->UpdateWorldSpaceState(FRigidTransform3(DynamicCube->GetP(), DynamicCube->GetQ()), FVec3(0));
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
if (Collisions.NumConstraints() <= 0)
{
return;
}
FPBDCollisionConstraint& Constraint = Collisions.GetConstraint(0);
if (FPBDRigidParticleHandle* PBDRigid = Constraint.GetParticle0()->CastToRigidParticle())
{
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
Collisions.GatherInput(Dt);
EXPECT_EQ(Constraint.GetParticle0(), DynamicCube);
EXPECT_EQ(Constraint.GetParticle1(), StaticCube);
EXPECT_NEAR(Constraint.CalculateWorldContactNormal().Z, FReal(1), KINDA_SMALL_NUMBER);
{
INVARIANT_XR_START(DynamicCube);
Collisions.Apply(Dt, 1);
INVARIANT_XR_END(DynamicCube);
}
// If we need accurate restitution we need more velocity iterations
Collisions.ApplyPushOut(Dt, 4);
Collisions.ScatterOutput(Dt);
// This test's tolerances are set to be very crude as to not be over sensitive (for now)
EXPECT_NEAR(DynamicCube->GetV().Z, 50.0f, 5.0f); // restitution not too low
EXPECT_TRUE(FMath::Abs(DynamicCube->GetV().X) < 1.0f);
EXPECT_TRUE(FMath::Abs(DynamicCube->GetV().Y) < 1.0f);
}
void CollisionBoxToStaticBox()
{
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)0;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs Particles(UniqueIndices);
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto StaticBox = AppendStaticParticleBox(Particles);
StaticBox->SetX(FVec3(-0.05f, -0.05f, -0.1f));
StaticBox->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
StaticBox->UpdateWorldSpaceState(FRigidTransform3(StaticBox->GetX(), StaticBox->GetR()), FVec3(0));
FReal Dt = FReal(1) / FReal(24.);
auto Box2 = AppendDynamicParticleBox(Particles);
FVec3 StartingPoint(0.5f);
Box2->SetP(StartingPoint);
Box2->SetQ(Box2->GetR());
Box2->SetV(FVec3(0, 0, -1));
Box2->SetPreV(Box2->GetV());
Box2->SetX(Box2->GetP() - Box2->GetV() * Dt);
Box2->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
Box2->UpdateWorldSpaceState(FRigidTransform3(Box2->GetP(), Box2->GetQ()), FVec3(0));
FBox Region(FVector(FReal(.2)), FVector(FReal(.5)));
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 1, 1);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
Collisions.GatherInput(Dt);
FPBDCollisionConstraint& Constraint = Collisions.GetConstraint(0);
if (auto PBDRigid = Constraint.GetParticle0()->CastToRigidParticle())
{
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
EXPECT_EQ(Constraint.GetParticle0(), Box2);
EXPECT_EQ(Constraint.GetParticle1(), StaticBox);
EXPECT_TRUE(Constraint.CalculateWorldContactNormal().Equals(FVector(0.0, 0.0, 1.0f)));
EXPECT_TRUE(FMath::Abs(Constraint.GetPhi() - FReal(-0.4)) < SMALL_THRESHOLD);
EXPECT_TRUE(FMath::Abs(Box2->GetV().Size() - 1.f)<SMALL_THRESHOLD ); // no velocity change yet
{
INVARIANT_XR_START(Box2);
INVARIANT_XR_START(StaticBox);
Collisions.Apply(Dt, 1);
INVARIANT_XR_END(Box2);
INVARIANT_XR_END(StaticBox);
}
RESET_PQ(Box2);
{
//INVARIANT_XR_START(Box2);
//INVARIANT_XR_START(StaticBox);
//INVARIANT_VW_START(Box2);
const int32 NumIts = 10;
Collisions.ApplyPushOut(Dt, NumIts);
//INVARIANT_XR_END(Box2);
//INVARIANT_XR_END(StaticBox);
//INVARIANT_VW_END(Box2);
}
Collisions.ScatterOutput(Dt);
EXPECT_TRUE(Box2->GetV().Size() < FVector(0, -1, 0).Size()); // slowed down
EXPECT_TRUE(Box2->GetW().Size() > 0); // now has rotation
EXPECT_FALSE(Box2->GetP().Equals(StartingPoint)); // moved
EXPECT_FALSE(Box2->GetQ().Equals(FQuat::Identity)); // and rotated
}
void CollisionCapsuleCapsule()
{
const FReal CullDistance = FReal(3);
{
FCapsule Capsule1(FVec3(-20, -10, 10), FVec3(20, -10, 10), FReal(20));
FCapsule Capsule2(FVec3(-20, -10, 10), FVec3(20, -10, 10), FReal(20));
FImplicitObjectPtr Implicit1 = MakeImplicitObjectPtr<FCapsule>(Capsule1);
FImplicitObjectPtr Implicit2 = MakeImplicitObjectPtr<FCapsule>(Capsule2);
FRigidTransform3 Transform1(FRigidTransform3::Identity);
FRigidTransform3 Transform2(FVec3(0.0, 0.0, 18), FRotation3(FQuat::Identity));
FPBDCollisionConstraint Constraint;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs RigidsSOA(UniqueIndices);
TArray<FPBDRigidParticleHandle*> Particles = RigidsSOA.CreateDynamicParticles(2);
FGeometryParticleHandle* Particle1 = Particles[0];
FGeometryParticleHandle* Particle2 = Particles[1];
Particle1->SetGeometry(Implicit1);
Particle2->SetGeometry(Implicit2);
const FShapeInstancePtr& Shape1 = Particle1->ShapeInstances()[0];
const FShapeInstancePtr& Shape2 = Particle2->ShapeInstances()[0];
FPBDCollisionConstraint::Make(Particle1,
Implicit1,
Shape1.Get(),
nullptr,
Transform1,
Particle1,
Implicit2,
Shape2.Get(),
nullptr,
Transform2,
CullDistance,
true,
EContactShapesType::CapsuleCapsule,
Constraint);
Collisions::UpdateConstraint(Constraint, Transform1, Transform2, 0.1);
const TArrayView<FManifoldPoint>& ManifoldPoints = Constraint.GetManifoldPoints();
EXPECT_EQ(ManifoldPoints.Num(), 2);
EXPECT_EQ(ManifoldPoints[0].ContactPoint.Phi, -22.0f); // Two radius 40 minus 18 of translation -> -22.0f
EXPECT_EQ(ManifoldPoints[0].ContactPoint.ShapeContactPoints[0], FVec3f(20, -10, 30)); // z = 10 + 20 radius -> 30.0f
EXPECT_EQ(ManifoldPoints[0].ContactPoint.ShapeContactPoints[1], FVec3f(20, -10, -10));
EXPECT_EQ(ManifoldPoints[1].ContactPoint.Phi, -18.0f);
EXPECT_EQ(ManifoldPoints[1].ContactPoint.ShapeContactPoints[0], FVec3f(-20, -10, 30));
EXPECT_EQ(ManifoldPoints[1].ContactPoint.ShapeContactPoints[1], FVec3f(-20, -10, 30));
{
Transform1.SetRotation(FRotation3::FromAxisAngle(FVec3(1.0, 0.0, 0.0), 0.1));
Collisions::UpdateConstraint(Constraint, Transform1, Transform2, 0.1);
const TArrayView<FManifoldPoint>& ManifoldPoints2 = Constraint.GetManifoldPoints();
EXPECT_EQ(ManifoldPoints2.Num(), 2);
EXPECT_LT(ManifoldPoints2[0].ContactPoint.Phi, -20.0f);
EXPECT_GT(ManifoldPoints2[0].ContactPoint.Phi, -21.0f);
EXPECT_LT(ManifoldPoints2[1].ContactPoint.Phi, -19.0f);
EXPECT_GT(ManifoldPoints2[1].ContactPoint.Phi, -20.0f);
}
}
{
FCapsule Capsule1(FVec3(-20, -10, 10), FVec3(20, -10, 10), FReal(18));
FCapsule Capsule2(FVec3(-20, -10, 10), FVec3(20, -10, 10), FReal(20));
FImplicitObjectPtr Implicit1 = MakeImplicitObjectPtr<FCapsule>(Capsule1);
FImplicitObjectPtr Implicit2 = MakeImplicitObjectPtr<FCapsule>(Capsule2);
FRigidTransform3 Transform1(FRigidTransform3::Identity);
FRigidTransform3 Transform2(FVec3(0.0, 0.0, 16), FRotation3(FQuat::Identity));
FPBDCollisionConstraint Constraint;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs RigidsSOA(UniqueIndices);
TArray<FPBDRigidParticleHandle*> Particles = RigidsSOA.CreateDynamicParticles(2);
FGeometryParticleHandle* Particle1 = Particles[0];
FGeometryParticleHandle* Particle2 = Particles[1];
Particle1->SetGeometry(Implicit1);
Particle2->SetGeometry(Implicit2);
const FShapeInstancePtr& Shape1 = Particle1->ShapeInstances()[0];
const FShapeInstancePtr& Shape2 = Particle2->ShapeInstances()[0];
FPBDCollisionConstraint::Make(Particle1,
Implicit1,
Shape1.Get(),
nullptr,
Transform1,
Particle1,
Implicit2,
Shape2.Get(),
nullptr,
Transform2,
CullDistance,
true,
EContactShapesType::CapsuleCapsule,
Constraint);
Collisions::UpdateConstraint(Constraint, Transform1, Transform2, 0.1);
const TArrayView<FManifoldPoint>& ManifoldPoints = Constraint.GetManifoldPoints();
EXPECT_EQ(ManifoldPoints.Num(), 2);
EXPECT_EQ(ManifoldPoints[0].ContactPoint.Phi, -22.0f);
EXPECT_EQ(ManifoldPoints[0].ContactPoint.ShapeContactPoints[0], FVec3f(20, -10, 28));
EXPECT_EQ(ManifoldPoints[0].ContactPoint.ShapeContactPoints[1], FVec3f(20, -10, -10));
EXPECT_EQ(ManifoldPoints[1].ContactPoint.Phi, -18.0f);
EXPECT_EQ(ManifoldPoints[1].ContactPoint.ShapeContactPoints[0], FVec3f(-20, -10, 28));
EXPECT_EQ(ManifoldPoints[1].ContactPoint.ShapeContactPoints[1], FVec3f(-20, -10, 30));
}
{
FCapsule Capsule1(FVec3(-20, -10, 10), FVec3(20, -10, 10), FReal(20));
FCapsule Capsule2(FVec3(-20, -10, 10), FVec3(20, -10, 10), FReal(18));
FImplicitObjectPtr Implicit1 = MakeImplicitObjectPtr<FCapsule>(Capsule1);
FImplicitObjectPtr Implicit2 = MakeImplicitObjectPtr<FCapsule>(Capsule2);
FRigidTransform3 Transform1(FRigidTransform3::Identity);
FRigidTransform3 Transform2(FVec3(0.0, 0.0, 16), FRotation3(FQuat::Identity));
FPBDCollisionConstraint Constraint;
FParticleUniqueIndicesMultithreaded UniqueIndices;
FPBDRigidsSOAs RigidsSOA(UniqueIndices);
TArray<FPBDRigidParticleHandle*> Particles = RigidsSOA.CreateDynamicParticles(2);
FGeometryParticleHandle* Particle1 = Particles[0];
FGeometryParticleHandle* Particle2 = Particles[1];
Particle1->SetGeometry(Implicit1);
Particle2->SetGeometry(Implicit2);
const FShapeInstancePtr& Shape1 = Particle1->ShapeInstances()[0];
const FShapeInstancePtr& Shape2 = Particle2->ShapeInstances()[0];
FPBDCollisionConstraint::Make(Particle1,
Implicit1,
Shape1.Get(),
nullptr,
Transform1,
Particle1,
Implicit2,
Shape2.Get(),
nullptr,
Transform2,
CullDistance,
true,
EContactShapesType::CapsuleCapsule,
Constraint);
Collisions::UpdateConstraint(Constraint, Transform1, Transform2, 0.1);
const TArrayView<FManifoldPoint>& ManifoldPoints = Constraint.GetManifoldPoints();
EXPECT_EQ(ManifoldPoints.Num(), 2);
EXPECT_EQ(ManifoldPoints[0].ContactPoint.Phi, -22.0f);
EXPECT_EQ(ManifoldPoints[0].ContactPoint.ShapeContactPoints[0], FVec3f(20, -10, 30));
EXPECT_EQ(ManifoldPoints[0].ContactPoint.ShapeContactPoints[1], FVec3f(20, -10, -8));
EXPECT_EQ(ManifoldPoints[1].ContactPoint.Phi, -18.0f);
EXPECT_EQ(ManifoldPoints[1].ContactPoint.ShapeContactPoints[0], FVec3f(-20, -10, -10));
EXPECT_EQ(ManifoldPoints[1].ContactPoint.ShapeContactPoints[1], FVec3f(-20, -10, -8));
}
}
}
Reference in New Issue View Git Blame Copy Permalink