izzy/UnrealEngineUWP
0
0
Fork 0
mirror of https://github.com/izzy2lost/UnrealEngineUWP.git synced 2026-03-26 18:15:20 -07:00
Code Issues Packages Projects Releases Wiki Activity
Files
00a43bb11c45cfb2e07fa0e79d0eb7bb7ef04c19
UnrealEngineUWP/Engine/Source/Programs/HeadlessChaos/Private/HeadlessChaosTestCollisions.cpp
Andrew Davidson 3ddc3a4da3 Merge up from //UE5/Dev-LargeWorldCoordinates 
#rb none

[CL 16211417 by Andrew Davidson in ue5-main branch]
2021-05-05 15:07:25 -04:00

675 lines
26 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/PBDCollisionConstraintsPGS.h"
#include "Chaos/CollisionResolutionTypes.h"
#include "Chaos/Sphere.h"
#include "Chaos/Utilities.h"
#include "Modules/ModuleManager.h"
#define SMALL_THRESHOLD 1e-4
#define RESET_PQ(Particle) Particle->P() = Particle->X(); Particle->Q() = Particle->R()
#define INVARIANT_XR_START(Particle) FVec3 InvariantPreX_##Particle = Particle->X(); FRotation3 InvariantPreR_##Particle = Particle->R()
#define INVARIANT_XR_END(Particle) EXPECT_TRUE(InvariantPreX_##Particle.Equals(Particle->X())); EXPECT_TRUE(InvariantPreR_##Particle.Equals(Particle->R()))
#define INVARIANT_VW_START(Particle) FVec3 InvariantPreV_##Particle = Particle->V(); FVec3 InvariantPreW_##Particle = Particle->W()
#define INVARIANT_VW_END(Particle) EXPECT_TRUE(InvariantPreV_##Particle.Equals(Particle->V())); EXPECT_TRUE(InvariantPreW_##Particle.Equals(Particle->W()))
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;
FPBDRigidsSOAs Particles;
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto Box1 = AppendDynamicParticleBox(Particles);
Box1->X() = FVec3(1.f);
Box1->R() = FRotation3(FQuat::Identity);
Box1->P() = Box1->X();
Box1->Q() = Box1->R();
Box1->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
auto Box2 = AppendDynamicParticleBox(Particles);
Box2->X() = FVec3(1.5f, 1.5f, 1.9f);
Box2->R() = FRotation3(FQuat::Identity);
Box2->P() = Box2->X();
Box2->Q() = Box2->R();
Box2->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 1, 1);
Collisions.ComputeConstraints(0.f);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FCollisionConstraintBase& Constraint = Collisions.GetConstraint(0);
if (auto PBDRigid = Constraint.Particle[0]->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.template As<FPBDCollisionConstraints::FPointContactConstraint>());
EXPECT_EQ(Constraint.Particle[0], Box2);
EXPECT_EQ(Constraint.Particle[1], Box1);
EXPECT_TRUE(Constraint.GetNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(ChaosTest::SignedDistance(*Constraint.Particle[0], Constraint.GetLocation())) < SMALL_THRESHOLD);
}
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;
FPBDRigidsSOAs Particles;
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto Box1 = AppendDynamicParticleConvexBox(Particles, FVec3(1.f) );
Box1->X() = FVec3(0.f);
Box1->R() = FRotation3(FQuat::Identity);
Box1->P() = Box1->X();
Box1->Q() = Box1->R();
Box1->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
auto Box2 = AppendDynamicParticleBox(Particles, FVec3(1.f) );
Box2->X() = FVec3(1.25f, 0.f, 0.f);
Box2->R() = FRotation3(FQuat::Identity);
Box2->P() = Box2->X();
Box2->Q() = Box2->R();
Box2->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 1, 1);
Collisions.ComputeConstraints(0.f);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FCollisionConstraintBase & Constraint = Collisions.GetConstraint(0);
Collisions.UpdateLevelsetConstraint(*Constraint.template As<FPBDCollisionConstraints::FPointContactConstraint>());
EXPECT_EQ(Constraint.Particle[0], Box2);
EXPECT_EQ(Constraint.Particle[1], Box1);
EXPECT_TRUE(Constraint.GetNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(ChaosTest::SignedDistance(*Constraint.Particle[0], Constraint.GetLocation())) < SMALL_THRESHOLD);
}
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;
FPBDRigidsSOAs Particles;
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto Floor = AppendStaticAnalyticFloor(Particles);
auto Box = AppendDynamicParticleBox(Particles);
Box->X() = FVec3(0, 1, 0);
Box->R() = FRotation3(FQuat::Identity);
Box->V() = FVec3(0, 0, -1);
Box->PreV() = Box->V();
Box->P() = Box->X();
Box->Q() = Box->R();
Box->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
const FReal Dt = 1 / 24.;
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FCollisionConstraintBase & Constraint = Collisions.GetConstraint(0);
if (auto PBDRigid = Constraint.Particle[0]->CastToRigidParticle())
{
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
Collisions.UpdateLevelsetConstraint(*Constraint.template As<FPBDCollisionConstraints::FPointContactConstraint>());
EXPECT_EQ(Constraint.Particle[0], Box);
EXPECT_EQ(Constraint.Particle[1], Floor);
EXPECT_TRUE(Constraint.GetNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(ChaosTest::SignedDistance(*Constraint.Particle[0], Constraint.GetLocation())) < SMALL_THRESHOLD);
EXPECT_TRUE(FMath::Abs(Constraint.GetPhi() - FReal(-0.5) ) < SMALL_THRESHOLD );
{
INVARIANT_XR_START(Box);
const int32 NumIts = 1;
for (int32 It = 0; It < NumIts; ++It)
{
Collisions.Apply(Dt, { Collisions.GetConstraintHandle(0) }, It, NumIts);
}
INVARIANT_XR_END(Box);
}
// Velocity is below the restitution threshold, so expecting 0 velocity despite the fact that restitution is 1
EXPECT_TRUE(Box->V().Equals(FVec3(0)));
EXPECT_TRUE(Box->W().Equals(FVec3(0)));
{
RESET_PQ(Box);
{
INVARIANT_XR_START(Box);
INVARIANT_VW_START(Box);
const int32 NumIts = 10;
for (int32 It = 0; It < NumIts; ++It)
{
Collisions.ApplyPushOut(Dt, { Collisions.GetConstraintHandle(0) }, TSet<const FGeometryParticleHandle*>(), It, NumIts);
}
INVARIANT_XR_END(Box);
INVARIANT_VW_END(Box);
}
}
EXPECT_NEAR(Box->P().Z, 0.5f, 1.e-2f);
}
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;
FPBDRigidsSOAs Particles;
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto Floor = AppendStaticConvexFloor(Particles);
auto Box = AppendDynamicParticleConvexBox( Particles, FVec3(50) );
Box->X() = FVec3(0, 0, 49);
Box->R() = FRotation3(FQuat::Identity);
Box->V() = FVec3(0, 0, -1);
Box->PreV() = Box->V();
Box->P() = Box->X();
Box->Q() = Box->R();
Box->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
const FReal Dt = 1 / 24.;
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FRigidBodyPointContactConstraint * Constraint = Collisions.GetConstraint(0).template As<FRigidBodyPointContactConstraint>();
EXPECT_TRUE(Constraint != nullptr);
Collisions.Update(*Constraint);
EXPECT_EQ(Constraint->Particle[0], Box);
EXPECT_EQ(Constraint->Particle[1], Floor);
EXPECT_TRUE(Constraint->GetNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs( Constraint->GetLocation().Z - FVec3(0,0,-1).Z ) < SMALL_THRESHOLD);
EXPECT_TRUE(FMath::Abs(Constraint->GetPhi() - FReal(-1.0)) < SMALL_THRESHOLD);
{
INVARIANT_XR_START(Box);
Collisions.Apply(Dt, { Collisions.GetConstraintHandle(0) }, 0, 1);
INVARIANT_XR_END(Box);
}
// 0 restitution so expecting 0 velocity
//EXPECT_TRUE(Box->V().Equals(FVec3(0)));
//EXPECT_TRUE(Box->W().Equals(FVec3(0)));
{
RESET_PQ(Box);
{
INVARIANT_XR_START(Box);
INVARIANT_VW_START(Box);
const int32 NumIts = 10;
for (int32 It = 0; It < NumIts; ++It)
{
Collisions.ApplyPushOut(Dt, { Collisions.GetConstraintHandle(0) }, TSet<const FGeometryParticleHandle*>(), It, NumIts);
}
INVARIANT_XR_END(Box);
INVARIANT_VW_END(Box);
}
}
//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;
FPBDRigidsSOAs Particles;
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto Floor = AppendStaticAnalyticFloor(Particles);
auto Box = AppendDynamicParticleBox(Particles);
Box->X() = FVec3(0, 1, 0);
Box->R() = FRotation3(FQuat::Identity);
Box->V() = FVec3(0, 0, -1);
Box->PreV() = Box->V();
Box->P() = Box->X();
Box->Q() = Box->R();
Box->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
const FReal Dt = 1 / 24.;
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FCollisionConstraintBase & Constraint = Collisions.GetConstraint(0);
if (auto PBDRigid = Constraint.Particle[0]->CastToRigidParticle())
{
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
Collisions.UpdateLevelsetConstraint(*Constraint.template As<FPBDCollisionConstraints::FPointContactConstraint>());
EXPECT_EQ(Constraint.Particle[0], Box);
EXPECT_EQ(Constraint.Particle[1], Floor);
EXPECT_TRUE(Constraint.GetNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(ChaosTest::SignedDistance(*Constraint.Particle[0], Constraint.GetLocation())) < SMALL_THRESHOLD);
EXPECT_TRUE(FMath::Abs(Constraint.GetPhi() - FReal(-0.5)) < SMALL_THRESHOLD);
{
INVARIANT_XR_START(Box);
Collisions.Apply(Dt, { Collisions.GetConstraintHandle(0) }, 0, 1);
INVARIANT_XR_END(Box);
}
// 0 restitution so expecting 0 velocity
EXPECT_TRUE(Box->V().Equals(FVec3(0)));
EXPECT_TRUE(Box->W().Equals(FVec3(0)));
{
RESET_PQ(Box);
{
INVARIANT_XR_START(Box);
INVARIANT_VW_START(Box);
const int32 NumIts = 10;
for (int32 It = 0; It < NumIts; ++It)
{
Collisions.ApplyPushOut(Dt, { Collisions.GetConstraintHandle(0) }, TSet<const FGeometryParticleHandle*>(), It, NumIts);
}
INVARIANT_XR_END(Box);
INVARIANT_VW_END(Box);
}
}
EXPECT_TRUE(FVec3::IsNearlyEqual(Box->P(), FVector(0.f, 1.f, 0.5f), 1.e-2f));
}
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;
FPBDRigidsSOAs Particles;
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto Floor = AppendStaticAnalyticFloor(Particles);
auto Box = AppendDynamicParticleBox(Particles);
Box->X() = FVec3(0, 0, 0);
Box->R() = FRotation3(FQuat::Identity);
Box->V() = FVec3(0, 0, -100);
Box->PreV() = Box->V();
Box->P() = Box->X();
Box->Q() = Box->R();
Box->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
const FReal Dt = 1 / 24.;
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FCollisionConstraintBase & Constraint = Collisions.GetConstraint(0);
if (auto PBDRigid = Constraint.Particle[0]->CastToRigidParticle())
{
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
Collisions.UpdateLevelsetConstraint(*Constraint.template As<FPBDCollisionConstraints::FPointContactConstraint>());
EXPECT_EQ(Constraint.Particle[0], Box);
EXPECT_EQ(Constraint.Particle[1], Floor);
EXPECT_TRUE(Constraint.GetNormal().operator==(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(ChaosTest::SignedDistance(*Constraint.Particle[0], Constraint.GetLocation())) < SMALL_THRESHOLD);
EXPECT_TRUE(FMath::Abs(Constraint.GetPhi() - FReal(-0.5)) < SMALL_THRESHOLD);
{
INVARIANT_XR_START(Box);
Collisions.Apply(Dt, { Collisions.GetConstraintHandle(0) }, 0, 1);
INVARIANT_XR_END(Box);
}
// full restitution, so expecting negative velocity
EXPECT_TRUE(Box->V().Equals(FVec3(0.f, 0.f, 100.f)));
EXPECT_TRUE(Box->W().Equals(FVec3(0)));
// collision occurs before full dt takes place, so need some bounce back for the remaining time we have
//EXPECT_TRUE(Particles.P(BoxId).Equals(Particles.X(BoxId)));
//EXPECT_TRUE(Particles.Q(BoxId).Equals(Particles.R(BoxId)));
{
RESET_PQ(Box);
{
INVARIANT_XR_START(Box);
const int32 NumIts = 10;
for (int32 It = 0; It < NumIts; ++It)
{
Collisions.ApplyPushOut(Dt, { Collisions.GetConstraintHandle(0) }, TSet<const FGeometryParticleHandle*>(), It, NumIts);
}
INVARIANT_XR_END(Box);
}
}
//for push out velocity is unimportant, so expecting simple pop out
EXPECT_TRUE(FVec3::IsNearlyEqual(Box->P(), FVector(0.f, 0.f, 0.5f), 1.e-2f));
EXPECT_TRUE(Box->Q().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
// The dynamic cube will collide with one of its vertices onto a face of the static cube
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;
FPBDRigidsSOAs Particles;
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
FGeometryParticleHandle* StaticCube = AppendStaticParticleBox(Particles, FVec3(100.0f));
StaticCube->X() = FVec3(0, 0, -50.0f);
FPBDRigidParticleHandle* DynamicCube = AppendDynamicParticleBox(Particles, FVec3(100.0f));
DynamicCube->X() = FVec3(0, 0, 80); // Penetrating by about 5cm
DynamicCube->R() = FRotation3::FromElements( 0.27059805f, 0.27059805f, 0.0f, 0.923879532f ); // Rotate so that vertex collide
DynamicCube->V() = FVec3(0, 0, -100);
DynamicCube->PreV() = DynamicCube->V();
DynamicCube->P() = DynamicCube->X();
DynamicCube->Q() = DynamicCube->R();
DynamicCube->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
const FReal Dt = 1 / 24.;
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 2, 5);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
if (Collisions.NumConstraints() <= 0)
{
return;
}
FCollisionConstraintBase& Constraint = Collisions.GetConstraint(0);
if (FPBDRigidParticleHandle* PBDRigid = Constraint.Particle[0]->CastToRigidParticle())
{
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
Collisions.UpdateLevelsetConstraint(*Constraint.template As<FPBDCollisionConstraints::FPointContactConstraint>());
EXPECT_EQ(Constraint.Particle[0], DynamicCube);
EXPECT_EQ(Constraint.Particle[1], StaticCube);
EXPECT_TRUE(Constraint.GetNormal().Equals(FVec3(0, 0, 1)));
EXPECT_TRUE(FMath::Abs(ChaosTest::SignedDistance(*Constraint.Particle[0], Constraint.GetLocation())) < SMALL_THRESHOLD);
{
INVARIANT_XR_START(DynamicCube);
Collisions.Apply(Dt, { Collisions.GetConstraintHandle(0) }, 0, 1);
INVARIANT_XR_END(DynamicCube);
}
// This test's tolerances are set to be very crude as to not be over sensitive (for now)
EXPECT_TRUE(DynamicCube->V().Z > 10.0f); // restitution not too low
EXPECT_TRUE(DynamicCube->V().Z < 70.0f); // restitution not too high
EXPECT_TRUE(FMath::Abs(DynamicCube->V().X) < 1.0f);
EXPECT_TRUE(FMath::Abs(DynamicCube->V().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;
FPBDRigidsSOAs Particles;
Particles.GetParticleHandles().AddArray(&Collided);
Particles.GetParticleHandles().AddArray(&PhysicsMaterials);
Particles.GetParticleHandles().AddArray(&PerParticlePhysicsMaterials);
auto StaticBox = AppendStaticParticleBox(Particles);
StaticBox->X() = FVec3(-0.05f, -0.05f, -0.1f);
StaticBox->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
auto Box2 = AppendDynamicParticleBox(Particles);
FVec3 StartingPoint(0.5f);
Box2->X() = StartingPoint;
Box2->P() = Box2->X();
Box2->Q() = Box2->R();
Box2->V() = FVec3(0, 0, -1);
Box2->PreV() = Box2->V();
Box2->AuxilaryValue(PhysicsMaterials) = MakeSerializable(PhysicsMaterial);
FBox Region(FVector(.2), FVector(.5));
FReal Dt = 1 / 24.;
FPBDCollisionConstraintAccessor Collisions(Particles, Collided, PhysicsMaterials, PerParticlePhysicsMaterials, 1, 1);
Collisions.ComputeConstraints(Dt);
EXPECT_EQ(Collisions.NumConstraints(), 1);
FCollisionConstraintBase & Constraint = Collisions.GetConstraint(0);
Collisions.Update(Constraint);
//Collisions.UpdateLevelsetConstraintGJK(Particles, Constraint);
//EXPECT_EQ(Constraint.ParticleIndex, 1);
//EXPECT_EQ(Constraint.LevelsetIndex, 0);
//EXPECT_TRUE(Constraint.GetNormal().Equals(FVector(0.0, 1.0, 0.0f))); // GJK returns a different result!
//EXPECT_TRUE(FMath::Abs(ChaosTest::SignedDistance(Particles, Constraint.ParticleIndex, Constraint.GetLocation())) < SMALL_THRESHOLD);
//EXPECT_TRUE(FMath::Abs(Constraint.GetPhi() - T(-0.233)) < SMALL_THRESHOLD);
if (auto PBDRigid = Constraint.Particle[0]->CastToRigidParticle())
{
PBDRigid->CollisionParticles()->UpdateAccelerationStructures();
}
EXPECT_EQ(Constraint.Particle[0], Box2);
EXPECT_EQ(Constraint.Particle[1], StaticBox);
EXPECT_TRUE(Constraint.GetNormal().Equals(FVector(0.0, 0.0, 1.0f)));
EXPECT_TRUE(FMath::Abs(ChaosTest::SignedDistance(*Constraint.Particle[0], Constraint.GetLocation())) < SMALL_THRESHOLD);
EXPECT_TRUE(FMath::Abs(Constraint.GetPhi() - FReal(-0.4)) < SMALL_THRESHOLD);
EXPECT_TRUE(FMath::Abs(Box2->V().Size() - 1.f)<SMALL_THRESHOLD ); // no velocity change yet
{
INVARIANT_XR_START(Box2);
INVARIANT_XR_START(StaticBox);
Collisions.Apply(Dt, { Collisions.GetConstraintHandle(0) }, 0, 1);
INVARIANT_XR_END(Box2);
INVARIANT_XR_END(StaticBox);
}
EXPECT_TRUE(Box2->V().Size()<FVector(0,-1,0).Size()); // slowed down
EXPECT_TRUE(Box2->W().Size()>0); // now has rotation
RESET_PQ(Box2);
{
//INVARIANT_XR_START(Box2);
//INVARIANT_XR_START(StaticBox);
//INVARIANT_VW_START(Box2);
const int32 NumIts = 10;
for (int32 It = 0; It < NumIts; ++It)
{
Collisions.ApplyPushOut(Dt, { Collisions.GetConstraintHandle(0) }, TSet<const FGeometryParticleHandle*>(), It, NumIts);
}
//INVARIANT_XR_END(Box2);
//INVARIANT_XR_END(StaticBox);
//INVARIANT_VW_END(Box2);
}
EXPECT_FALSE(Box2->P().Equals(StartingPoint)); // moved
EXPECT_FALSE(Box2->Q().Equals(FQuat::Identity)); // and rotated
}
void CollisionPGS()
{
#if CHAOS_PARTICLEHANDLE_TODO
TSet<int32> ActiveIndices;
TArray<TSet<int32>> IslandParticles;
TArray<int32> IslandSleepCounts;
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)0;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
Chaos::FPBDRigidParticles Particles;
Particles.AddArray(&Collided);
Particles.AddArray(&PhysicsMaterials);
int32 BoxId1 = AppendParticleBox<FReal>(Particles);
Particles.X(BoxId1) = FVec3(0.f, 0.f, 0.5f);
Particles.R(BoxId1) = FRotation3(FQuat::Identity);
Particles.V(BoxId1) = FVec3(0.f, 0.f, -10.f);
PhysicsMaterials[BoxId1] = MakeSerializable(PhysicsMaterial);
int32 BoxId2 = AppendParticleBox<FReal>(Particles);
Particles.X(BoxId2) = FVec3(0.f, 0.f, 0.5f);
Particles.R(BoxId2) = FRotation3(FQuat::Identity);
Particles.V(BoxId2) = FVec3(0.f, 0.f, -10.f);
PhysicsMaterials[BoxId2] = MakeSerializable(PhysicsMaterial);
int32 FloorId = AppendStaticAnalyticFloor<FReal>(Particles);
ActiveIndices.Add(BoxId1);
ActiveIndices.Add(BoxId2);
ActiveIndices.Add(FloorId);
TArray<int32> Indices = ActiveIndices.Array();
FPBDCollisionConstraintPGS CollisionConstraints(Particles, Indices, Collided, PhysicsMaterials);
FRigidBodyContactConstraintPGS Constraint1;
Constraint1.ParticleIndex = BoxId1;
Constraint1.LevelsetIndex = BoxId2;
FRigidBodyContactConstraintPGS Constraint2;
Constraint2.ParticleIndex = BoxId1;
Constraint2.LevelsetIndex = FloorId;
FRigidBodyContactConstraintPGS Constraint3;
Constraint3.ParticleIndex = BoxId2;
Constraint3.LevelsetIndex = FloorId;
CollisionConstraints.Constraints.Add(Constraint1);
CollisionConstraints.Constraints.Add(Constraint2);
CollisionConstraints.Constraints.Add(Constraint3);
CollisionConstraints.Apply(Particles, 1.f, { 0, 1, 2 });
EXPECT_LT(FMath::Abs(Particles.V(BoxId1)[2]), 1e-3);
EXPECT_LT(FMath::Abs(Particles.V(BoxId2)[2]), 1e-3);
#endif
}
void CollisionPGS2()
{
#if CHAOS_PARTICLEHANDLE_TODO
TSet<int32> ActiveIndices;
TArray<TSet<int32>> IslandParticles;
TArray<int32> IslandSleepCounts;
TArrayCollectionArray<bool> Collided;
TUniquePtr<FChaosPhysicsMaterial> PhysicsMaterial = MakeUnique<FChaosPhysicsMaterial>();
PhysicsMaterial->Friction = (FReal)0;
PhysicsMaterial->Restitution = (FReal)0;
TArrayCollectionArray<TSerializablePtr<FChaosPhysicsMaterial>> PhysicsMaterials;
TArrayCollectionArray<TUniquePtr<FChaosPhysicsMaterial>> PerParticlePhysicsMaterials;
Chaos::FPBDRigidParticles Particles;
Particles.AddArray(&Collided);
Particles.AddArray(&PhysicsMaterials);
int32 BoxId1 = AppendParticleBox<FReal>(Particles);
Particles.X(BoxId1) = FVec3(0.f, 0.f, 0.5f);
Particles.R(BoxId1) = FRotation3(FQuat::Identity);
Particles.V(BoxId1) = FVec3(0.f, 0.f, -10.f);
PhysicsMaterials[BoxId1] = MakeSerializable(PhysicsMaterial);
int32 BoxId2 = AppendParticleBox<FReal>(Particles);
Particles.X(BoxId2) = FVec3(0.f, 0.f, 0.5f);
Particles.R(BoxId2) = FRotation3(FQuat::Identity);
Particles.V(BoxId2) = FVec3(0.f, 0.f, -10.f);
PhysicsMaterials[BoxId2] = MakeSerializable(PhysicsMaterial);
int32 FloorId = AppendStaticAnalyticFloor<FReal>(Particles);
ActiveIndices.Add(BoxId1);
ActiveIndices.Add(BoxId2);
ActiveIndices.Add(FloorId);
TArray<int32> Indices = ActiveIndices.Array();
FPBDCollisionConstraintPGS CollisionConstraints(Particles, Indices, Collided, PhysicsMaterials);
FRigidBodyContactConstraintPGS Constraint1;
Constraint1.ParticleIndex = BoxId1;
Constraint1.LevelsetIndex = BoxId2;
FRigidBodyContactConstraintPGS Constraint2;
Constraint2.ParticleIndex = BoxId1;
Constraint2.LevelsetIndex = FloorId;
FRigidBodyContactConstraintPGS Constraint3;
Constraint3.ParticleIndex = BoxId2;
Constraint3.LevelsetIndex = FloorId;
CollisionConstraints.Constraints.Add(Constraint2);
CollisionConstraints.Constraints.Add(Constraint3);
CollisionConstraints.Constraints.Add(Constraint1);
CollisionConstraints.Apply(Particles, 1.f, { 0, 1, 2 });
EXPECT_LT(FMath::Abs(Particles.V(BoxId1)[2] - 0.5), 1e-3);
EXPECT_LT(FMath::Abs(Particles.V(BoxId2)[2] + 0.5), 1e-3);
#endif
}
}
Reference in New Issue View Git Blame Copy Permalink