75 lines
2.5 KiB
C++
75 lines
2.5 KiB
C++
|
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
||
|
|
// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
|
||
|
|
// SPDX-License-Identifier: MIT
|
||
|
|
|
||
|
|
#include "UnitTestFramework.h"
|
||
|
|
#include "PhysicsTestContext.h"
|
||
|
|
#include <Jolt/Physics/Constraints/DistanceConstraint.h>
|
||
|
|
#include "Layers.h"
|
||
|
|
|
||
|
|
TEST_SUITE("DistanceConstraintTests")
|
||
|
|
{
|
||
|
|
// Test if the distance constraint can be used to create a spring
|
||
|
|
TEST_CASE("TestDistanceSpring")
|
||
|
|
{
|
||
|
|
// Configuration of the spring
|
||
|
|
const RVec3 cInitialPosition(10, 0, 0);
|
||
|
|
const float cFrequency = 2.0f;
|
||
|
|
const float cDamping = 0.1f;
|
||
|
|
|
||
|
|
for (int mode = 0; mode < 2; ++mode)
|
||
|
|
{
|
||
|
|
// Create a sphere
|
||
|
|
PhysicsTestContext context;
|
||
|
|
context.ZeroGravity();
|
||
|
|
Body &body = context.CreateSphere(cInitialPosition, 0.5f, EMotionType::Dynamic, EMotionQuality::Discrete, Layers::MOVING);
|
||
|
|
body.GetMotionProperties()->SetLinearDamping(0.0f);
|
||
|
|
|
||
|
|
// Calculate stiffness and damping of spring
|
||
|
|
float m = 1.0f / body.GetMotionProperties()->GetInverseMass();
|
||
|
|
float omega = 2.0f * JPH_PI * cFrequency;
|
||
|
|
float k = m * Square(omega);
|
||
|
|
float c = 2.0f * m * cDamping * omega;
|
||
|
|
|
||
|
|
// Create spring
|
||
|
|
DistanceConstraintSettings constraint;
|
||
|
|
constraint.mPoint2 = cInitialPosition;
|
||
|
|
if (mode == 0)
|
||
|
|
{
|
||
|
|
// First iteration use stiffness and damping
|
||
|
|
constraint.mLimitsSpringSettings.mMode = ESpringMode::StiffnessAndDamping;
|
||
|
|
constraint.mLimitsSpringSettings.mStiffness = k;
|
||
|
|
constraint.mLimitsSpringSettings.mDamping = c;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// Second iteration use frequency and damping
|
||
|
|
constraint.mLimitsSpringSettings.mMode = ESpringMode::FrequencyAndDamping;
|
||
|
|
constraint.mLimitsSpringSettings.mFrequency = cFrequency;
|
||
|
|
constraint.mLimitsSpringSettings.mDamping = cDamping;
|
||
|
|
}
|
||
|
|
constraint.mMinDistance = constraint.mMaxDistance = 0.0f;
|
||
|
|
context.CreateConstraint<DistanceConstraint>(Body::sFixedToWorld, body, constraint);
|
||
|
|
|
||
|
|
// Simulate spring
|
||
|
|
Real x = cInitialPosition.GetX();
|
||
|
|
float v = 0.0f;
|
||
|
|
float dt = context.GetDeltaTime();
|
||
|
|
for (int i = 0; i < 120; ++i)
|
||
|
|
{
|
||
|
|
// Using the equations from page 32 of Soft Constraints: Reinventing The Spring - Erin Catto - GDC 2011 for an implicit euler spring damper
|
||
|
|
v = (v - dt * k / m * float(x)) / (1.0f + dt * c / m + Square(dt) * k / m);
|
||
|
|
x += v * dt;
|
||
|
|
|
||
|
|
// Run physics simulation
|
||
|
|
context.SimulateSingleStep();
|
||
|
|
|
||
|
|
// Test if simulation matches prediction
|
||
|
|
CHECK_APPROX_EQUAL(x, body.GetPosition().GetX(), 5.0e-6_r);
|
||
|
|
CHECK(body.GetPosition().GetY() == 0);
|
||
|
|
CHECK(body.GetPosition().GetZ() == 0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|