Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
godotengine
GitHub Repository: godotengine/godot
 Path: blob/master/thirdparty/jolt_physics/Jolt/Physics/Constraints/TwoBodyConstraint.cpp
9913 views
1
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)

2
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe

3
// SPDX-License-Identifier: MIT

4


5
#include <Jolt/Jolt.h>

6


7
#include <Jolt/Physics/Constraints/TwoBodyConstraint.h>

8
#include <Jolt/Physics/IslandBuilder.h>

9
#include <Jolt/Physics/LargeIslandSplitter.h>

10
#include <Jolt/Physics/Body/BodyManager.h>

11


12
#ifdef JPH_DEBUG_RENDERER

13
	#include <Jolt/Renderer/DebugRenderer.h>

14
#endif // JPH_DEBUG_RENDERER

15


16
JPH_NAMESPACE_BEGIN

17


18
JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(TwoBodyConstraintSettings)

19
{

20
	JPH_ADD_BASE_CLASS(TwoBodyConstraintSettings, ConstraintSettings)

21
}

22


23
void TwoBodyConstraint::BuildIslands(uint32 inConstraintIndex, IslandBuilder &ioBuilder, BodyManager &inBodyManager)

24
{

25
	// Activate bodies

26
	BodyID body_ids[2];

27
	int num_bodies = 0;

28
	if (mBody1->IsDynamic() && !mBody1->IsActive())

29
		body_ids[num_bodies++] = mBody1->GetID();

30
	if (mBody2->IsDynamic() && !mBody2->IsActive())

31
		body_ids[num_bodies++] = mBody2->GetID();

32
	if (num_bodies > 0)

33
		inBodyManager.ActivateBodies(body_ids, num_bodies);

34


35
	// Link the bodies into the same island

36
	ioBuilder.LinkConstraint(inConstraintIndex, mBody1->GetIndexInActiveBodiesInternal(), mBody2->GetIndexInActiveBodiesInternal());

37
}

38


39
uint TwoBodyConstraint::BuildIslandSplits(LargeIslandSplitter &ioSplitter) const

40
{

41
	return ioSplitter.AssignSplit(mBody1, mBody2);

42
}

43


44
#ifdef JPH_DEBUG_RENDERER

45


46
void TwoBodyConstraint::DrawConstraintReferenceFrame(DebugRenderer *inRenderer) const

47
{

48
	RMat44 transform1 = mBody1->GetCenterOfMassTransform() * GetConstraintToBody1Matrix();

49
	RMat44 transform2 = mBody2->GetCenterOfMassTransform() * GetConstraintToBody2Matrix();

50
	inRenderer->DrawCoordinateSystem(transform1, 1.1f * mDrawConstraintSize);

51
	inRenderer->DrawCoordinateSystem(transform2, mDrawConstraintSize);

52
}

53


54
#endif // JPH_DEBUG_RENDERER

55


56
JPH_NAMESPACE_END

57


58