113 lines
3.3 KiB
C++
113 lines
3.3 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#include "HeadlessChaos.h"
|
|
#include "HeadlessChaosTestUtility.h"
|
|
|
|
#include "Chaos/CollisionResolutionTypes.h"
|
|
#include "Chaos/Evolution/SolverBody.h"
|
|
#include "Chaos/GJK.h"
|
|
#include "Chaos/Pair.h"
|
|
#include "Chaos/PBDRigidsEvolution.h"
|
|
#include "Chaos/PBDRigidParticles.h"
|
|
#include "Chaos/Sphere.h"
|
|
#include "Chaos/Utilities.h"
|
|
|
|
namespace ChaosTest
|
|
{
|
|
using namespace Chaos;
|
|
|
|
class FCollisionSolverTest : public ::testing::Test
|
|
{
|
|
protected:
|
|
FCollisionSolverTest()
|
|
{
|
|
|
|
}
|
|
|
|
~FCollisionSolverTest()
|
|
{
|
|
}
|
|
};
|
|
|
|
#if 0
|
|
|
|
// We can create and initialze a constraint with simple parameters and it does something sensible
|
|
// Create 1 static body and 1 dynamic body, separated along Z
|
|
// Create a contact between them with some overlap
|
|
// Check that the penetration is resolved correctly
|
|
// Single contact point, no rotations, etc
|
|
TEST_F(FCollisionSolverTest, TestBasicContact)
|
|
{
|
|
const FReal Dt;
|
|
FSolverBody Bodies[2];
|
|
FPBDCollisionSolver Collision;
|
|
|
|
const FReal Radius = 50.0f;
|
|
const FReal Overlap = 10.0f;
|
|
|
|
const FVec3 X0 = FVec3(0);
|
|
const FRotation3 R0 = FRotation3::FromIdentity();
|
|
const FVec3 V0 = FVec3(0);
|
|
const FVec3 W0 = FVec3(0);
|
|
|
|
const FVec3 X1 = FVec3(0,0,2 * Radius - Overlap);
|
|
const FRotation3 R1 = FRotation3::FromIdentity();
|
|
const FVec3 V1 = FVec3(0);
|
|
const FVec3 W1 = FVec3(0);
|
|
const FReal M1 = 100.0f;
|
|
const FReal I1 = 100.0f * 1000.0f; // Approx box size 100
|
|
|
|
// Static body
|
|
Bodies[0].SetX(X0 - V0 * Dt));
|
|
Bodies[0].SetP(X0);
|
|
Bodies[0].SetR(FRotation3::IntegrateRotationWithAngularVelocity(R0, -W0, Dt));
|
|
Bodies[0].SetQ(R0);
|
|
Bodies[0].SetV(V0);
|
|
Bodies[0].SetW(W0);
|
|
|
|
// Dynamic body
|
|
Bodies[1].SetX(X1 - V1 * Dt));
|
|
Bodies[1].SetP(X1);
|
|
Bodies[1].SetR(FRotation3::IntegrateRotationWithAngularVelocity(R1, -W1, Dt));
|
|
Bodies[1].SetQ(R1);
|
|
Bodies[1].SetV(V1);
|
|
Bodies[1].SetW(W1);
|
|
Bodies[1].SetInvM(FReal(1) / M1);
|
|
Bodies[1].SetInvI(FReal(1) / I1);
|
|
|
|
|
|
Collision.SetSolverBodies(&Bodies[0], &Bodies[1]);
|
|
Collision.SetFriction(0, 0, 0, 0);
|
|
Collision.SetNumManifoldPoints(1);
|
|
Collision.InitContact(0, FVec3(0,0,Radius), FVec3(0,0,-Radius), FVec3(0,0,1));
|
|
Collision.InitMaterial(0, 0, false, 0);
|
|
|
|
Collision.SolvePosition(Dt, false);
|
|
|
|
// Bodies should be separated and not rotated
|
|
EXPECT_NEAR(Bodies[1].P().X, FReal(0), KINDA_SMALL_NUMBER);
|
|
EXPECT_NEAR(Bodies[1].P().Y, FReal(0), KINDA_SMALL_NUMBER);
|
|
EXPECT_NEAR(Bodies[1].P().Z, FReal(100), KINDA_SMALL_NUMBER);
|
|
EXPECT_NEAR(Bodies[1].Q().X, FReal(0), KINDA_SMALL_NUMBER);
|
|
EXPECT_NEAR(Bodies[1].Q().Y, FReal(0), KINDA_SMALL_NUMBER);
|
|
EXPECT_NEAR(Bodies[1].Q().Z, FReal(0), KINDA_SMALL_NUMBER);
|
|
EXPECT_NEAR(Bodies[1].Q().W, FReal(1), KINDA_SMALL_NUMBER);
|
|
|
|
Bodies[1].SetImplicitVelocity(Dt);
|
|
|
|
// Dynamic body should have gained Z velocity because it started penetrating
|
|
EXPECT_NEAR(Bodies[1].V().X, FReal(0), KINDA_SMALL_NUMBER);
|
|
EXPECT_NEAR(Bodies[1].V().Y, FReal(0)), KINDA_SMALL_NUMBER;
|
|
EXPECT_NEAR(Bodies[1].V().Z, Overlap * Dt, KINDA_SMALL_NUMBER);
|
|
|
|
Collision.SolveVelocity(Dt, false);
|
|
|
|
// Dynamic body should have 0 Z velocity because we resolved Restitution constraint
|
|
EXPECT_NEAR(Bodies[1].V().X, FReal(0), KINDA_SMALL_NUMBER);
|
|
EXPECT_NEAR(Bodies[1].V().Y, FReal(0)), KINDA_SMALL_NUMBER;
|
|
EXPECT_NEAR(Bodies[1].V().Z, FReal(0), KINDA_SMALL_NUMBER);
|
|
}
|
|
|
|
#endif
|
|
}
|