Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
godotengine
GitHub Repository: godotengine/godot
 Path: blob/master/thirdparty/jolt_physics/Jolt/Skeleton/SkeletonPose.cpp
9912 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/Skeleton/SkeletonPose.h>

8
#ifdef JPH_DEBUG_RENDERER

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

10
#endif // JPH_DEBUG_RENDERER

11


12
JPH_NAMESPACE_BEGIN

13


14
void SkeletonPose::SetSkeleton(const Skeleton *inSkeleton)

15
{

16
	mSkeleton = inSkeleton;

17


18
	mJoints.resize(mSkeleton->GetJointCount());

19
	mJointMatrices.resize(mSkeleton->GetJointCount());

20
}

21


22
void SkeletonPose::CalculateJointMatrices()

23
{

24
	for (int i = 0; i < (int)mJoints.size(); ++i)

25
	{

26
		mJointMatrices[i] = mJoints[i].ToMatrix();

27


28
		int parent = mSkeleton->GetJoint(i).mParentJointIndex;

29
		if (parent >= 0)

30
		{

31
			JPH_ASSERT(parent < i, "Joints must be ordered: parents first");

32
			mJointMatrices[i] = mJointMatrices[parent] * mJointMatrices[i];

33
		}

34
	}

35
}

36


37
void SkeletonPose::CalculateJointStates()

38
{

39
	for (int i = 0; i < (int)mJoints.size(); ++i)

40
	{

41
		Mat44 local_transform;

42
		int parent = mSkeleton->GetJoint(i).mParentJointIndex;

43
		if (parent >= 0)

44
			local_transform = mJointMatrices[parent].Inversed() * mJointMatrices[i];

45
		else

46
			local_transform = mJointMatrices[i];

47


48
		JointState &joint = mJoints[i];

49
		joint.mTranslation = local_transform.GetTranslation();

50
		joint.mRotation = local_transform.GetQuaternion();

51
	}

52
}

53


54
void SkeletonPose::CalculateLocalSpaceJointMatrices(Mat44 *outMatrices) const

55
{

56
	for (int i = 0; i < (int)mJoints.size(); ++i)

57
		outMatrices[i] = mJoints[i].ToMatrix();

58
}

59


60
#ifdef JPH_DEBUG_RENDERER

61
void SkeletonPose::Draw(const DrawSettings &inDrawSettings, DebugRenderer *inRenderer, RMat44Arg inOffset) const

62
{

63
	RMat44 offset = inOffset * RMat44::sTranslation(mRootOffset);

64


65
	const Skeleton::JointVector &joints = mSkeleton->GetJoints();

66


67
	for (int b = 0; b < mSkeleton->GetJointCount(); ++b)

68
	{

69
		RMat44 joint_transform = offset * mJointMatrices[b];

70


71
		if (inDrawSettings.mDrawJoints)

72
		{

73
			int parent = joints[b].mParentJointIndex;

74
			if (parent >= 0)

75
				inRenderer->DrawLine(offset * mJointMatrices[parent].GetTranslation(), joint_transform.GetTranslation(), Color::sGreen);

76
		}

77


78
		if (inDrawSettings.mDrawJointOrientations)

79
			inRenderer->DrawCoordinateSystem(joint_transform, 0.05f);

80


81
		if (inDrawSettings.mDrawJointNames)

82
			inRenderer->DrawText3D(joint_transform.GetTranslation(), joints[b].mName, Color::sWhite, 0.05f);

83
	}

84
}

85
#endif // JPH_DEBUG_RENDERER

86


87
JPH_NAMESPACE_END

88


89