CoCalc -- ScaledShape.cpp

GitHub Repository: godotengine/godot
Path: blob/master/thirdparty/jolt_physics/Jolt/Physics/Collision/Shape/ScaledShape.cpp
⁹⁹¹³ 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/ScaledShape.h>
8
#include <Jolt/Physics/Collision/Shape/ScaleHelpers.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/Physics/Collision/CollisionDispatch.h>
13
#include <Jolt/ObjectStream/TypeDeclarations.h>
14
#include <Jolt/Core/StreamIn.h>
15
#include <Jolt/Core/StreamOut.h>
16

17
JPH_NAMESPACE_BEGIN
18

19
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(ScaledShapeSettings)
20
{
21
	JPH_ADD_BASE_CLASS(ScaledShapeSettings, DecoratedShapeSettings)
22

23
	JPH_ADD_ATTRIBUTE(ScaledShapeSettings, mScale)
24
}
25

26
ShapeSettings::ShapeResult ScaledShapeSettings::Create() const
27
{
28
	if (mCachedResult.IsEmpty())
29
		Ref<Shape> shape = new ScaledShape(*this, mCachedResult);
30
	return mCachedResult;
31
}
32

33
ScaledShape::ScaledShape(const ScaledShapeSettings &inSettings, ShapeResult &outResult) :
34
	DecoratedShape(EShapeSubType::Scaled, inSettings, outResult),
35
	mScale(inSettings.mScale)
36
{
37
	if (outResult.HasError())
38
		return;
39

40
	if (ScaleHelpers::IsZeroScale(inSettings.mScale))
41
	{
42
		outResult.SetError("Can't use zero scale!");
43
		return;
44
	}
45

46
	outResult.Set(this);
47
}
48

49
MassProperties ScaledShape::GetMassProperties() const
50
{
51
	MassProperties p = mInnerShape->GetMassProperties();
52
	p.Scale(mScale);
53
	return p;
54
}
55

56
AABox ScaledShape::GetLocalBounds() const
57
{
58
	return mInnerShape->GetLocalBounds().Scaled(mScale);
59
}
60

61
AABox ScaledShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
62
{
63
	return mInnerShape->GetWorldSpaceBounds(inCenterOfMassTransform, inScale * mScale);
64
}
65

66
TransformedShape ScaledShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
67
{
68
	// We don't use any bits in the sub shape ID
69
	outRemainder = inSubShapeID;
70

71
	TransformedShape ts(RVec3(inPositionCOM), inRotation, mInnerShape, BodyID());
72
	ts.SetShapeScale(inScale * mScale);
73
	return ts;
74
}
75

76
Vec3 ScaledShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
77
{
78
	// Transform the surface point to local space and pass the query on
79
	Vec3 normal = mInnerShape->GetSurfaceNormal(inSubShapeID, inLocalSurfacePosition / mScale);
80

81
	// Need to transform the plane normals using inScale
82
	// Transforming a direction with matrix M is done through multiplying by (M^-1)^T
83
	// In this case M is a diagonal matrix with the scale vector, so we need to multiply our normal by 1 / scale and renormalize afterwards
84
	return (normal / mScale).Normalized();
85
}
86

87
void ScaledShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
88
{
89
	mInnerShape->GetSupportingFace(inSubShapeID, inDirection, inScale * mScale, inCenterOfMassTransform, outVertices);
90
}
91

92
void ScaledShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, RVec3Arg inBaseOffset)) const
93
{
94
	mInnerShape->GetSubmergedVolume(inCenterOfMassTransform, inScale * mScale, inSurface, outTotalVolume, outSubmergedVolume, outCenterOfBuoyancy JPH_IF_DEBUG_RENDERER(, inBaseOffset));
95
}
96

97
#ifdef JPH_DEBUG_RENDERER
98
void ScaledShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
99
{
100
	mInnerShape->Draw(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inUseMaterialColors, inDrawWireframe);
101
}
102

103
void ScaledShape::DrawGetSupportFunction(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
104
{
105
	mInnerShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform, inScale * mScale, inColor, inDrawSupportDirection);
106
}
107

108
void ScaledShape::DrawGetSupportingFace(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
109
{
110
	mInnerShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform, inScale * mScale);
111
}
112
#endif // JPH_DEBUG_RENDERER
113

114
bool ScaledShape::CastRay(const RayCast &inRay, const SubShapeIDCreator &inSubShapeIDCreator, RayCastResult &ioHit) const
115
{
116
	Vec3 inv_scale = mScale.Reciprocal();
117
	RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection };
118
	return mInnerShape->CastRay(scaled_ray, inSubShapeIDCreator, ioHit);
119
}
120

121
void ScaledShape::CastRay(const RayCast &inRay, const RayCastSettings &inRayCastSettings, const SubShapeIDCreator &inSubShapeIDCreator, CastRayCollector &ioCollector, const ShapeFilter &inShapeFilter) const
122
{
123
	// Test shape filter
124
	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
125
		return;
126

127
	Vec3 inv_scale = mScale.Reciprocal();
128
	RayCast scaled_ray { inv_scale * inRay.mOrigin, inv_scale * inRay.mDirection };
129
	return mInnerShape->CastRay(scaled_ray, inRayCastSettings, inSubShapeIDCreator, ioCollector, inShapeFilter);
130
}
131

132
void ScaledShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
133
{
134
	// Test shape filter
135
	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
136
		return;
137

138
	Vec3 inv_scale = mScale.Reciprocal();
139
	mInnerShape->CollidePoint(inv_scale * inPoint, inSubShapeIDCreator, ioCollector, inShapeFilter);
140
}
141

142
void ScaledShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
143
{
144
	mInnerShape->CollideSoftBodyVertices(inCenterOfMassTransform, inScale * mScale, inVertices, inNumVertices, inCollidingShapeIndex);
145
}
146

147
void ScaledShape::CollectTransformedShapes(const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, const SubShapeIDCreator &inSubShapeIDCreator, TransformedShapeCollector &ioCollector, const ShapeFilter &inShapeFilter) const
148
{
149
	// Test shape filter
150
	if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
151
		return;
152

153
	mInnerShape->CollectTransformedShapes(inBox, inPositionCOM, inRotation, inScale * mScale, inSubShapeIDCreator, ioCollector, inShapeFilter);
154
}
155

156
void ScaledShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
157
{
158
	mInnerShape->TransformShape(inCenterOfMassTransform * Mat44::sScale(mScale), ioCollector);
159
}
160

161
void ScaledShape::SaveBinaryState(StreamOut &inStream) const
162
{
163
	DecoratedShape::SaveBinaryState(inStream);
164

165
	inStream.Write(mScale);
166
}
167

168
void ScaledShape::RestoreBinaryState(StreamIn &inStream)
169
{
170
	DecoratedShape::RestoreBinaryState(inStream);
171

172
	inStream.Read(mScale);
173
}
174

175
float ScaledShape::GetVolume() const
176
{
177
	return abs(mScale.GetX() * mScale.GetY() * mScale.GetZ()) * mInnerShape->GetVolume();
178
}
179

180
bool ScaledShape::IsValidScale(Vec3Arg inScale) const
181
{
182
	return mInnerShape->IsValidScale(inScale * mScale);
183
}
184

185
Vec3 ScaledShape::MakeScaleValid(Vec3Arg inScale) const
186
{
187
	return mInnerShape->MakeScaleValid(mScale * inScale) / mScale;
188
}
189

190
void ScaledShape::sCollideScaledVsShape(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)
191
{
192
	JPH_ASSERT(inShape1->GetSubType() == EShapeSubType::Scaled);
193
	const ScaledShape *shape1 = static_cast<const ScaledShape *>(inShape1);
194

195
	CollisionDispatch::sCollideShapeVsShape(shape1->GetInnerShape(), inShape2, inScale1 * shape1->GetScale(), inScale2, inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
196
}
197

198
void ScaledShape::sCollideShapeVsScaled(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)
199
{
200
	JPH_ASSERT(inShape2->GetSubType() == EShapeSubType::Scaled);
201
	const ScaledShape *shape2 = static_cast<const ScaledShape *>(inShape2);
202

203
	CollisionDispatch::sCollideShapeVsShape(inShape1, shape2->GetInnerShape(), inScale1, inScale2 * shape2->GetScale(), inCenterOfMassTransform1, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, inCollideShapeSettings, ioCollector, inShapeFilter);
204
}
205

206
void ScaledShape::sCastScaledVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
207
{
208
	JPH_ASSERT(inShapeCast.mShape->GetSubType() == EShapeSubType::Scaled);
209
	const ScaledShape *shape = static_cast<const ScaledShape *>(inShapeCast.mShape);
210

211
	ShapeCast scaled_cast(shape->GetInnerShape(), inShapeCast.mScale * shape->GetScale(), inShapeCast.mCenterOfMassStart, inShapeCast.mDirection);
212
	CollisionDispatch::sCastShapeVsShapeLocalSpace(scaled_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
213
}
214

215
void ScaledShape::sCastShapeVsScaled(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
216
{
217
	JPH_ASSERT(inShape->GetSubType() == EShapeSubType::Scaled);
218
	const ScaledShape *shape = static_cast<const ScaledShape *>(inShape);
219

220
	CollisionDispatch::sCastShapeVsShapeLocalSpace(inShapeCast, inShapeCastSettings, shape->mInnerShape, inScale * shape->mScale, inShapeFilter, inCenterOfMassTransform2, inSubShapeIDCreator1, inSubShapeIDCreator2, ioCollector);
221
}
222

223
void ScaledShape::sRegister()
224
{
225
	ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::Scaled);
226
	f.mConstruct = []() -> Shape * { return new ScaledShape; };
227
	f.mColor = Color::sYellow;
228

229
	for (EShapeSubType s : sAllSubShapeTypes)
230
	{
231
		CollisionDispatch::sRegisterCollideShape(EShapeSubType::Scaled, s, sCollideScaledVsShape);
232
		CollisionDispatch::sRegisterCollideShape(s, EShapeSubType::Scaled, sCollideShapeVsScaled);
233
		CollisionDispatch::sRegisterCastShape(EShapeSubType::Scaled, s, sCastScaledVsShape);
234
		CollisionDispatch::sRegisterCastShape(s, EShapeSubType::Scaled, sCastShapeVsScaled);
235
	}
236
}
237

238
JPH_NAMESPACE_END
239

240
Product

Resources

Company
