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

8
#include <Jolt/Physics/Collision/CollisionDispatch.h>

9
#include <Jolt/Physics/Collision/RayCast.h>

10
#include <Jolt/Physics/Collision/ShapeCast.h>

11
#include <Jolt/Physics/Collision/TransformedShape.h>

12
#include <Jolt/Core/StreamIn.h>

13
#include <Jolt/Core/StreamOut.h>

14
#include <Jolt/ObjectStream/TypeDeclarations.h>

15


16
JPH_NAMESPACE_BEGIN

17


18
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(OffsetCenterOfMassShapeSettings)

19
{

20
	JPH_ADD_BASE_CLASS(OffsetCenterOfMassShapeSettings, DecoratedShapeSettings)

21


22
	JPH_ADD_ATTRIBUTE(OffsetCenterOfMassShapeSettings, mOffset)

23
}

24


25
ShapeSettings::ShapeResult OffsetCenterOfMassShapeSettings::Create() const

26
{

27
	if (mCachedResult.IsEmpty())

28
		Ref<Shape> shape = new OffsetCenterOfMassShape(*this, mCachedResult);

29
	return mCachedResult;

30
}

31


32
OffsetCenterOfMassShape::OffsetCenterOfMassShape(const OffsetCenterOfMassShapeSettings &inSettings, ShapeResult &outResult) :

33
	DecoratedShape(EShapeSubType::OffsetCenterOfMass, inSettings, outResult),

34
	mOffset(inSettings.mOffset)

35
{

36
	if (outResult.HasError())

37
		return;

38


39
	outResult.Set(this);

40
}

41


42
AABox OffsetCenterOfMassShape::GetLocalBounds() const

43
{

44
	AABox bounds = mInnerShape->GetLocalBounds();

45
	bounds.mMin -= mOffset;

46
	bounds.mMax -= mOffset;

47
	return bounds;

48
}

49


50
AABox OffsetCenterOfMassShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const

51
{

52
	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale);

53
}

54


55
TransformedShape OffsetCenterOfMassShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const

56
{

57
	// We don't use any bits in the sub shape ID

58
	outRemainder = inSubShapeID;

59


60
	TransformedShape ts(RVec3(inPositionCOM - inRotation * (inScale * mOffset)), inRotation, mInnerShape, BodyID());

61
	ts.SetShapeScale(inScale);

62
	return ts;

63
}

64


65
Vec3 OffsetCenterOfMassShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const

66
{

67
	// Transform surface position to local space and pass call on

68
	return mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition + mOffset);

69
}

70


71
void OffsetCenterOfMassShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const

72
{

73
	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), outVertices);

74
}

75


76
void OffsetCenterOfMassShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const

77
{

78
	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));

79
}

80


81
#ifdef JPH_DEBUG_RENDERER

82
void OffsetCenterOfMassShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const

83
{

84
	mInnerShape->Draw(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inUseMaterialColors, inDrawWireframe);

85
}

86


87
void OffsetCenterOfMassShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const

88
{

89
	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inColor, inDrawSupportDirection);

90
}

91


92
void OffsetCenterOfMassShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const

93
{

94
	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale);

95
}

96
#endif // JPH_DEBUG_RENDERER

97


98
bool OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const

99
{

100
	// Transform the ray to local space

101
	RayCast ray = inRay;

102
	ray.mOrigin += mOffset;

103


104
	return mInnerShape->CastRay(ray, inSubShapeIDCreator, ioHit);

105
}

106


107
void OffsetCenterOfMassShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const

108
{

109
	// Test shape filter

110
	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))

111
		return;

112


113
	// Transform the ray to local space

114
	RayCast ray = inRay;

115
	ray.mOrigin += mOffset;

116


117
	return mInnerShape->CastRay(ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);

118
}

119


120
void OffsetCenterOfMassShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const

121
{

122
	// Test shape filter

123
	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))

124
		return;

125


126
	// Pass the point on to the inner shape in local space

127
	mInnerShape->CollidePoint(inPoint + mOffset, inSubShapeIDCreator, ioCollector, inShapeFilter);

128
}

129


130
void OffsetCenterOfMassShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const

131
{

132
	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform.PreTranslated(-inScale * mOffset), inScale, inVertices, inNumVertices, inCollidingShapeIndex);

133
}

134


135
void OffsetCenterOfMassShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const

136
{

137
	// Test shape filter

138
	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))

139
		return;

140


141
	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM - inRotation * (inScale * mOffset), inRotation, inScale, inSubShapeIDCreator, ioCollector, inShapeFilter);

142
}

143


144
void OffsetCenterOfMassShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const

145
{

146
	mInnerShape->TransformShape(inCenterOfMassTransform.PreTranslated(-mOffset), ioCollector);

147
}

148


149
void OffsetCenterOfMassShape::sCollideOffsetCenterOfMassVsShape(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)

150
{

151
	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::OffsetCenterOfMass);

152
	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShape1);

153


154
	CollisionDispatch::sCollideShapeVsShape(shape1->mInnerShape, inShape2, inScale1, inScale2, inCenterOfMassTransform1.PreTranslated(-inScale1 * shape1->mOffset), inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);

155
}

156


157
void OffsetCenterOfMassShape::sCollideShapeVsOffsetCenterOfMass(const Shape *inShape1, const Shape *inShape2, Vec3Arg inScale1, Vec3Arg inScale2, Mat44Arg inCenterOfMassTransform1, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, const CollideShapeSettings &inCollideShapeSettings, CollideShapeCollector &ioCollector, const ShapeFilter &inShapeFilter)

158
{

159
	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::OffsetCenterOfMass);

160
	const OffsetCenterOfMassShape *shape2 = static_cast<const OffsetCenterOfMassShape *>(inShape2);

161


162
	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->mInnerShape, inScale1, inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2.PreTranslated(-inScale2 * shape2->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);

163
}

164


165
void OffsetCenterOfMassShape::sCastOffsetCenterOfMassVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)

166
{

167
	// Fetch offset center of mass shape from cast shape

168
	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::OffsetCenterOfMass);

169
	const OffsetCenterOfMassShape *shape1 = static_cast<const OffsetCenterOfMassShape *>(inShapeCast.mShape);

170


171
	// Transform the shape cast and update the shape

172
	ShapeCast shape_cast(shape1->mInnerShape, inShapeCast.mScale, inShapeCast.mCenterOfMassStart.PreTranslated(-inShapeCast.mScale * shape1->mOffset), inShapeCast.mDirection);

173


174
	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);

175
}

176


177
void OffsetCenterOfMassShape::sCastShapeVsOffsetCenterOfMass(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)

178
{

179
	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::OffsetCenterOfMass);

180
	const OffsetCenterOfMassShape *shape = static_cast<const OffsetCenterOfMassShape *>(inShape);

181


182
	// Transform the shape cast

183
	ShapeCast shape_cast = inShapeCast.PostTransformed(Mat44::sTranslation(inScale * shape->mOffset));

184


185
	CollisionDispatch::sCastShapeVsShapeLocalSpace(shape_cast, inShapeCastSettings, shape->mInnerShape, inScale, inShapeFilter, inCenterOfMassTransform2.PreTranslated(-inScale * shape->mOffset), inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);

186
}

187


188
void OffsetCenterOfMassShape::SaveBinaryState(StreamOut &inStream) const

189
{

190
	DecoratedShape::SaveBinaryState(inStream);

191


192
	inStream.Write(mOffset);

193
}

194


195
void OffsetCenterOfMassShape::RestoreBinaryState(StreamIn &inStream)

196
{

197
	DecoratedShape::RestoreBinaryState(inStream);

198


199
	inStream.Read(mOffset);

200
}

201


202
void OffsetCenterOfMassShape::sRegister()

203
{

204
	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::OffsetCenterOfMass);

205
	f.mConstruct = []() -> Shape * { return new OffsetCenterOfMassShape; };

206
	f.mColor = Color::sCyan;

207


208
	for (EShapeSubType s : sAllSubShapeTypes)

209
	{

210
		CollisionDispatch::sRegisterCollideShape(EShapeSubType::OffsetCenterOfMass, s, sCollideOffsetCenterOfMassVsShape);

211
		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::OffsetCenterOfMass, sCollideShapeVsOffsetCenterOfMass);

212
		CollisionDispatch::sRegisterCastShape(EShapeSubType::OffsetCenterOfMass, s, sCastOffsetCenterOfMassVsShape);

213
		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::OffsetCenterOfMass, sCastShapeVsOffsetCenterOfMass);

214
	}

215
}

216


217
JPH_NAMESPACE_END

218


219