JoltPhysics/Samples/Tests/General/AllowedDOFsTest.cpp

// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
// SPDX-License-Identifier: MIT

#include <TestFramework.h>

#include <Tests/General/AllowedDOFsTest.h>
#include <Jolt/Physics/Collision/Shape/BoxShape.h>
#include <Jolt/Physics/Constraints/DistanceConstraint.h>
#include <Jolt/Physics/Body/BodyCreationSettings.h>
#include <Renderer/DebugRendererImp.h>
#include <Layers.h>

JPH_IMPLEMENT_RTTI_VIRTUAL(AllowedDOFsTest)
{
	JPH_ADD_BASE_CLASS(AllowedDOFsTest, Test)
}

void AllowedDOFsTest::Initialize()
{
	// Floor
	CreateFloor();

	Vec3 box_size(0.5f, 1.0f, 2.0f);
	RefConst<Shape> box_shape = new BoxShape(box_size);

	for (int allowed_dofs = 1; allowed_dofs <= 0b111111; ++allowed_dofs)
	{
		float x = -35.0f + 10.0f * (allowed_dofs & 0b111);
		float z = -35.0f + 10.0f * ((allowed_dofs >> 3) & 0b111);

		// Create body
		BodyCreationSettings bcs(box_shape, RVec3(x, 10, z), Quat::sIdentity(), EMotionType::Dynamic, Layers::MOVING);
		bcs.mAllowedDOFs = (EAllowedDOFs)allowed_dofs;
		BodyID id = mBodyInterface->CreateAndAddBody(bcs, EActivation::Activate);
		mBodies.push_back(id);

		// Create a constraint
		DistanceConstraintSettings dcs;
		dcs.mPoint1 = bcs.mPosition + Vec3(5, 5, 5);
		dcs.mPoint2 = bcs.mPosition + box_size;
		dcs.mMinDistance = 0.0f;
		dcs.mMaxDistance = sqrt(3.0f) * 5.0f + 1.0f;
		mPhysicsSystem->AddConstraint(mBodyInterface->CreateConstraint(&dcs, BodyID(), id));

		// Draw degrees of freedom
		String allowed_dofs_str = "";
		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::TranslationX) == EAllowedDOFs::TranslationX)
			allowed_dofs_str += "X ";
		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::TranslationY) == EAllowedDOFs::TranslationY)
			allowed_dofs_str += "Y ";
		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::TranslationZ) == EAllowedDOFs::TranslationZ)
			allowed_dofs_str += "Z ";
		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::RotationX) == EAllowedDOFs::RotationX)
			allowed_dofs_str += "RX ";
		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::RotationY) == EAllowedDOFs::RotationY)
			allowed_dofs_str += "RY ";
		if ((EAllowedDOFs(allowed_dofs) & EAllowedDOFs::RotationZ) == EAllowedDOFs::RotationZ)
			allowed_dofs_str += "RZ ";
		SetBodyLabel(id, allowed_dofs_str);
	}
}