78 lines
3.1 KiB
C++
78 lines
3.1 KiB
C++
// Copyright Epic Games, Inc. All Rights Reserved.
|
|
|
|
#include "HeadlessChaos.h"
|
|
#include "HeadlessChaosTestJoint.h"
|
|
#include "Modules/ModuleManager.h"
|
|
|
|
namespace ChaosTest {
|
|
|
|
using namespace Chaos;
|
|
|
|
// One kinematic, one dynamic particle arrange horizontally.
|
|
// Locked position constraint and limited swing constraint
|
|
template <typename TEvolution>
|
|
void JointRestitution_Cone()
|
|
{
|
|
const int32 NumSolverIterations = 10;
|
|
const FReal Gravity = 0.0f;
|
|
const FReal Dt = 0.01f;
|
|
|
|
FJointChainTest<TEvolution> Test(NumSolverIterations, Gravity);
|
|
|
|
FPBDJointSolverSettings JointSolverSettings;
|
|
Test.Evolution.GetJointCombinedConstraints().LinearConstraints.SetSettings(JointSolverSettings);
|
|
|
|
Test.InitChain(2, FVec3(1,0,0));
|
|
|
|
const FReal SwingLimit = FMath::DegreesToRadians(30.0f);
|
|
Test.JointSettings[0].AngularMotionTypes = { EJointMotionType::Free, EJointMotionType::Limited, EJointMotionType::Limited };
|
|
Test.JointSettings[0].AngularLimits = { 0.0f, SwingLimit, SwingLimit };
|
|
Test.JointSettings[0].SwingRestitution = 1.0f;
|
|
Test.JointSettings[0].bProjectionEnabled = false;
|
|
|
|
|
|
Test.Create();
|
|
|
|
// Initialize the dynamic body as if it has rotated to the point that it will hit the swing limit at the next iteration (with a specific angular velocity)
|
|
const FReal AngVelY = FMath::DegreesToRadians(90.0f); // rad/s
|
|
const FReal HitTime = 0.5f;
|
|
const FVec3 Offset1 = Test.ParticlePositions[1] - Test.JointPositions[0];
|
|
const FRotation3 InitialRot1 = FRotation3::FromAxisAngle(FVec3(0,1,0), SwingLimit - HitTime * AngVelY * Dt);
|
|
const FVec3 InitialOffset1 = InitialRot1 * Offset1;
|
|
const FVec3 InitialPos1 = Test.JointPositions[0] + InitialOffset1;
|
|
const FVec3 InitialAngVel1 = FVec3(0.0f, AngVelY, 0.0f);
|
|
const FVec3 InitialVel1 = FVec3::CrossProduct(InitialAngVel1, InitialOffset1);
|
|
Test.ResetParticle(Test.GetParticle(1), InitialPos1, InitialRot1, InitialVel1, InitialAngVel1);
|
|
|
|
Test.Evolution.AdvanceOneTimeStep(Dt);
|
|
Test.Evolution.EndFrame(Dt);
|
|
|
|
// Check that the velocity and angular velocity was affected by restitution
|
|
if (auto KinParticle = Test.GetParticle(1)->CastToKinematicParticle())
|
|
{
|
|
const FVec3 ResultVel = KinParticle->GetV();
|
|
const FVec3 ResultAngVel = KinParticle->GetW();
|
|
|
|
const FVec3 ExpectedVel = -Test.JointSettings[0].SwingRestitution * InitialVel1;
|
|
const FVec3 ExpectedAngVel = -Test.JointSettings[0].SwingRestitution * InitialAngVel1;
|
|
|
|
// We need an insane number of iterations (~500) to get the velocity almost correct unless
|
|
// we do a matrix solve, but event though the linear velocity is not impacted enough,
|
|
// we still get decent results in practice because the angvel is correct.
|
|
//EXPECT_NEAR(ResultVel.X, ExpectedVel.X, 1.0f);
|
|
//EXPECT_NEAR(ResultVel.Y, ExpectedVel.Y, 1.0f);
|
|
//EXPECT_NEAR(ResultVel.Z, ExpectedVel.Z, 1.0f);
|
|
|
|
EXPECT_NEAR(ResultAngVel.X, ExpectedAngVel.X, 0.1f);
|
|
EXPECT_NEAR(ResultAngVel.Y, ExpectedAngVel.Y, 0.1f);
|
|
EXPECT_NEAR(ResultAngVel.Z, ExpectedAngVel.Z, 0.1f);
|
|
}
|
|
|
|
}
|
|
|
|
GTEST_TEST(AllEvolutions, DISABLED_JointRestitutionTests_Cone)
|
|
{
|
|
JointRestitution_Cone<FPBDRigidsEvolutionGBF>();
|
|
}
|
|
|
|
} |