CoCalc -- VehicleCollisionTester.cpp

GitHub Repository: godotengine/godot
Path: blob/master/thirdparty/jolt_physics/Jolt/Physics/Vehicle/VehicleCollisionTester.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/Vehicle/VehicleCollisionTester.h>
8
#include <Jolt/Physics/Vehicle/VehicleConstraint.h>
9
#include <Jolt/Physics/Collision/RayCast.h>
10
#include <Jolt/Physics/Collision/ShapeCast.h>
11
#include <Jolt/Physics/Collision/CastResult.h>
12
#include <Jolt/Physics/Collision/Shape/SphereShape.h>
13
#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
14
#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
15
#include <Jolt/Physics/PhysicsSystem.h>
16

17
JPH_NAMESPACE_BEGIN
18

19
bool VehicleCollisionTesterRay::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
20
{
21
	const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
22
	const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
23

24
	const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
25
	const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
26

27
	const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
28
	const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
29

30
	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
31
	float wheel_radius = wheel_settings->mRadius;
32
	float ray_length = wheel_settings->mSuspensionMaxLength + wheel_radius;
33
	RRayCast ray { inOrigin, ray_length * inDirection };
34

35
	class MyCollector : public CastRayCollector
36
	{
37
	public:
38
							MyCollector(PhysicsSystem &inPhysicsSystem, const RRayCast &inRay, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) :
39
			mPhysicsSystem(inPhysicsSystem),
40
			mRay(inRay),
41
			mUpDirection(inUpDirection),
42
			mCosMaxSlopeAngle(inCosMaxSlopeAngle)
43
		{
44
		}
45

46
		virtual void		AddHit(const RayCastResult &inResult) override
47
		{
48
			// Test if this collision is closer than the previous one
49
			if (inResult.mFraction < GetEarlyOutFraction())
50
			{
51
				// Lock the body
52
				BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID);
53
				JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
54
				const Body *body = &lock.GetBody();
55

56
				if (body->IsSensor())
57
					return;
58

59
				// Test that we're not hitting a vertical wall
60
				RVec3 contact_pos = mRay.GetPointOnRay(inResult.mFraction);
61
				Vec3 normal = body->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, contact_pos);
62
				if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle)
63
				{
64
					// Update early out fraction to this hit
65
					UpdateEarlyOutFraction(inResult.mFraction);
66

67
					// Get the contact properties
68
					mBody = body;
69
					mSubShapeID2 = inResult.mSubShapeID2;
70
					mContactPosition = contact_pos;
71
					mContactNormal = normal;
72
				}
73
			}
74
		}
75

76
		// Configuration
77
		PhysicsSystem &		mPhysicsSystem;
78
		RRayCast			mRay;
79
		Vec3				mUpDirection;
80
		float				mCosMaxSlopeAngle;
81

82
		// Resulting closest collision
83
		const Body *		mBody = nullptr;
84
		SubShapeID			mSubShapeID2;
85
		RVec3				mContactPosition;
86
		Vec3				mContactNormal;
87
	};
88

89
	RayCastSettings settings;
90

91
	MyCollector collector(inPhysicsSystem, ray, mUp, mCosMaxSlopeAngle);
92
	inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastRay(ray, settings, collector, broadphase_layer_filter, object_layer_filter, body_filter);
93
	if (collector.mBody == nullptr)
94
		return false;
95

96
	outBody = const_cast<Body *>(collector.mBody);
97
	outSubShapeID = collector.mSubShapeID2;
98
	outContactPosition = collector.mContactPosition;
99
	outContactNormal = collector.mContactNormal;
100
	outSuspensionLength = max(0.0f, ray_length * collector.GetEarlyOutFraction() - wheel_radius);
101

102
	return true;
103
}
104

105
void VehicleCollisionTesterRay::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
106
{
107
	// Recalculate the contact points assuming the contact point is on an infinite plane
108
	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
109
	float d_dot_n = inDirection.Dot(ioContactNormal);
110
	if (d_dot_n < -1.0e-6f)
111
	{
112
		// Reproject the contact position using the suspension ray and the plane formed by the contact position and normal
113
		ioContactPosition = inOrigin + Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal) / d_dot_n * inDirection;
114

115
		// The suspension length is simply the distance between the contact position and the suspension origin excluding the wheel radius
116
		ioSuspensionLength = Clamp(Vec3(ioContactPosition - inOrigin).Dot(inDirection) - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
117
	}
118
	else
119
	{
120
		// If the normal is pointing away we assume there's no collision anymore
121
		ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
122
	}
123
}
124

125
bool VehicleCollisionTesterCastSphere::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
126
{
127
	const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
128
	const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
129

130
	const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
131
	const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
132

133
	const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
134
	const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
135

136
	SphereShape sphere(mRadius);
137
	sphere.SetEmbedded();
138

139
	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
140
	float wheel_radius = wheel_settings->mRadius;
141
	float shape_cast_length = wheel_settings->mSuspensionMaxLength + wheel_radius - mRadius;
142
	RShapeCast shape_cast(&sphere, Vec3::sOne(), RMat44::sTranslation(inOrigin), inDirection * shape_cast_length);
143

144
	ShapeCastSettings settings;
145
	settings.mUseShrunkenShapeAndConvexRadius = true;
146
	settings.mReturnDeepestPoint = true;
147

148
	class MyCollector : public CastShapeCollector
149
	{
150
	public:
151
							MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast, Vec3Arg inUpDirection, float inCosMaxSlopeAngle) :
152
			mPhysicsSystem(inPhysicsSystem),
153
			mShapeCast(inShapeCast),
154
			mUpDirection(inUpDirection),
155
			mCosMaxSlopeAngle(inCosMaxSlopeAngle)
156
		{
157
		}
158

159
		virtual void		AddHit(const ShapeCastResult &inResult) override
160
		{
161
			// Test if this collision is closer/deeper than the previous one
162
			float early_out = inResult.GetEarlyOutFraction();
163
			if (early_out < GetEarlyOutFraction())
164
			{
165
				// Lock the body
166
				BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2);
167
				JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
168
				const Body *body = &lock.GetBody();
169

170
				if (body->IsSensor())
171
					return;
172

173
				// Test that we're not hitting a vertical wall
174
				Vec3 normal = -inResult.mPenetrationAxis.Normalized();
175
				if (normal.Dot(mUpDirection) > mCosMaxSlopeAngle)
176
				{
177
					// Update early out fraction to this hit
178
					UpdateEarlyOutFraction(early_out);
179

180
					// Get the contact properties
181
					mBody = body;
182
					mSubShapeID2 = inResult.mSubShapeID2;
183
					mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2;
184
					mContactNormal = normal;
185
					mFraction = inResult.mFraction;
186
				}
187
			}
188
		}
189

190
		// Configuration
191
		PhysicsSystem &		mPhysicsSystem;
192
		const RShapeCast &	mShapeCast;
193
		Vec3				mUpDirection;
194
		float				mCosMaxSlopeAngle;
195

196
		// Resulting closest collision
197
		const Body *		mBody = nullptr;
198
		SubShapeID			mSubShapeID2;
199
		RVec3				mContactPosition;
200
		Vec3				mContactNormal;
201
		float				mFraction;
202
	};
203

204
	MyCollector collector(inPhysicsSystem, shape_cast, mUp, mCosMaxSlopeAngle);
205
	inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter);
206
	if (collector.mBody == nullptr)
207
		return false;
208

209
	outBody = const_cast<Body *>(collector.mBody);
210
	outSubShapeID = collector.mSubShapeID2;
211
	outContactPosition = collector.mContactPosition;
212
	outContactNormal = collector.mContactNormal;
213
	outSuspensionLength = max(0.0f, shape_cast_length * collector.mFraction + mRadius - wheel_radius);
214

215
	return true;
216
}
217

218
void VehicleCollisionTesterCastSphere::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
219
{
220
	// Recalculate the contact points assuming the contact point is on an infinite plane
221
	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
222
	float d_dot_n = inDirection.Dot(ioContactNormal);
223
	if (d_dot_n < -1.0e-6f)
224
	{
225
		// Reproject the contact position using the suspension cast sphere and the plane formed by the contact position and normal
226
		// This solves x = inOrigin + fraction * inDirection and (x - ioContactPosition) . ioContactNormal = mRadius for fraction
227
		float oc_dot_n = Vec3(ioContactPosition - inOrigin).Dot(ioContactNormal);
228
		float fraction = (mRadius + oc_dot_n) / d_dot_n;
229
		ioContactPosition = inOrigin + fraction * inDirection - mRadius * ioContactNormal;
230

231
		// Calculate the new suspension length in the same way as the cast sphere normally does
232
		ioSuspensionLength = Clamp(fraction + mRadius - wheel_settings->mRadius, 0.0f, wheel_settings->mSuspensionMaxLength);
233
	}
234
	else
235
	{
236
		// If the normal is pointing away we assume there's no collision anymore
237
		ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
238
	}
239
}
240

241
bool VehicleCollisionTesterCastCylinder::Collide(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&outBody, SubShapeID &outSubShapeID, RVec3 &outContactPosition, Vec3 &outContactNormal, float &outSuspensionLength) const
242
{
243
	const DefaultBroadPhaseLayerFilter default_broadphase_layer_filter = inPhysicsSystem.GetDefaultBroadPhaseLayerFilter(mObjectLayer);
244
	const BroadPhaseLayerFilter &broadphase_layer_filter = mBroadPhaseLayerFilter != nullptr? *mBroadPhaseLayerFilter : default_broadphase_layer_filter;
245

246
	const DefaultObjectLayerFilter default_object_layer_filter = inPhysicsSystem.GetDefaultLayerFilter(mObjectLayer);
247
	const ObjectLayerFilter &object_layer_filter = mObjectLayerFilter != nullptr? *mObjectLayerFilter : default_object_layer_filter;
248

249
	const IgnoreSingleBodyFilter default_body_filter(inVehicleBodyID);
250
	const BodyFilter &body_filter = mBodyFilter != nullptr? *mBodyFilter : default_body_filter;
251

252
	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
253
	float max_suspension_length = wheel_settings->mSuspensionMaxLength;
254

255
	// Get the wheel transform given that the cylinder rotates around the Y axis
256
	RMat44 shape_cast_start = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX());
257
	shape_cast_start.SetTranslation(inOrigin);
258

259
	// Construct a cylinder with the dimensions of the wheel
260
	float wheel_half_width = 0.5f * wheel_settings->mWidth;
261
	CylinderShape cylinder(wheel_half_width, wheel_settings->mRadius, min(wheel_half_width, wheel_settings->mRadius) * mConvexRadiusFraction);
262
	cylinder.SetEmbedded();
263

264
	RShapeCast shape_cast(&cylinder, Vec3::sOne(), shape_cast_start, inDirection * max_suspension_length);
265

266
	ShapeCastSettings settings;
267
	settings.mUseShrunkenShapeAndConvexRadius = true;
268
	settings.mReturnDeepestPoint = true;
269

270
	class MyCollector : public CastShapeCollector
271
	{
272
	public:
273
							MyCollector(PhysicsSystem &inPhysicsSystem, const RShapeCast &inShapeCast) :
274
			mPhysicsSystem(inPhysicsSystem),
275
			mShapeCast(inShapeCast)
276
		{
277
		}
278

279
		virtual void		AddHit(const ShapeCastResult &inResult) override
280
		{
281
			// Test if this collision is closer/deeper than the previous one
282
			float early_out = inResult.GetEarlyOutFraction();
283
			if (early_out < GetEarlyOutFraction())
284
			{
285
				// Lock the body
286
				BodyLockRead lock(mPhysicsSystem.GetBodyLockInterfaceNoLock(), inResult.mBodyID2);
287
				JPH_ASSERT(lock.Succeeded()); // When this runs all bodies are locked so this should not fail
288
				const Body *body = &lock.GetBody();
289

290
				if (body->IsSensor())
291
					return;
292

293
				// Update early out fraction to this hit
294
				UpdateEarlyOutFraction(early_out);
295

296
				// Get the contact properties
297
				mBody = body;
298
				mSubShapeID2 = inResult.mSubShapeID2;
299
				mContactPosition = mShapeCast.mCenterOfMassStart.GetTranslation() + inResult.mContactPointOn2;
300
				mContactNormal = -inResult.mPenetrationAxis.Normalized();
301
				mFraction = inResult.mFraction;
302
			}
303
		}
304

305
		// Configuration
306
		PhysicsSystem &		mPhysicsSystem;
307
		const RShapeCast &	mShapeCast;
308

309
		// Resulting closest collision
310
		const Body *		mBody = nullptr;
311
		SubShapeID			mSubShapeID2;
312
		RVec3				mContactPosition;
313
		Vec3				mContactNormal;
314
		float				mFraction;
315
	};
316

317
	MyCollector collector(inPhysicsSystem, shape_cast);
318
	inPhysicsSystem.GetNarrowPhaseQueryNoLock().CastShape(shape_cast, settings, shape_cast.mCenterOfMassStart.GetTranslation(), collector, broadphase_layer_filter, object_layer_filter, body_filter);
319
	if (collector.mBody == nullptr)
320
		return false;
321

322
	outBody = const_cast<Body *>(collector.mBody);
323
	outSubShapeID = collector.mSubShapeID2;
324
	outContactPosition = collector.mContactPosition;
325
	outContactNormal = collector.mContactNormal;
326
	outSuspensionLength = max_suspension_length * collector.mFraction;
327

328
	return true;
329
}
330

331
void VehicleCollisionTesterCastCylinder::PredictContactProperties(PhysicsSystem &inPhysicsSystem, const VehicleConstraint &inVehicleConstraint, uint inWheelIndex, RVec3Arg inOrigin, Vec3Arg inDirection, const BodyID &inVehicleBodyID, Body *&ioBody, SubShapeID &ioSubShapeID, RVec3 &ioContactPosition, Vec3 &ioContactNormal, float &ioSuspensionLength) const
332
{
333
	// Recalculate the contact points assuming the contact point is on an infinite plane
334
	const WheelSettings *wheel_settings = inVehicleConstraint.GetWheel(inWheelIndex)->GetSettings();
335
	float d_dot_n = inDirection.Dot(ioContactNormal);
336
	if (d_dot_n < -1.0e-6f)
337
	{
338
		// Wheel size
339
		float half_width = 0.5f * wheel_settings->mWidth;
340
		float radius = wheel_settings->mRadius;
341

342
		// Get the inverse local space contact normal for a cylinder pointing along Y
343
		RMat44 wheel_transform = inVehicleConstraint.GetWheelWorldTransform(inWheelIndex, Vec3::sAxisY(), Vec3::sAxisX());
344
		Vec3 inverse_local_normal = -wheel_transform.Multiply3x3Transposed(ioContactNormal);
345

346
		// Get the support point of this normal in local space of the cylinder
347
		// See CylinderShape::Cylinder::GetSupport
348
		float x = inverse_local_normal.GetX(), y = inverse_local_normal.GetY(), z = inverse_local_normal.GetZ();
349
		float o = sqrt(Square(x) + Square(z));
350
		Vec3 support_point;
351
		if (o > 0.0f)
352
			support_point = Vec3((radius * x) / o, Sign(y) * half_width, (radius * z) / o);
353
		else
354
			support_point = Vec3(0, Sign(y) * half_width, 0);
355

356
		// Rotate back to world space
357
		support_point = wheel_transform.Multiply3x3(support_point);
358

359
		// Now we can use inOrigin + support_point as the start of a ray of our suspension to the contact plane
360
		// as know that it is the first point on the wheel that will hit the plane
361
		RVec3 origin = inOrigin + support_point;
362

363
		// Calculate contact position and suspension length, the is the same as VehicleCollisionTesterRay
364
		// but we don't need to take the radius into account anymore
365
		Vec3 oc(ioContactPosition - origin);
366
		ioContactPosition = origin + oc.Dot(ioContactNormal) / d_dot_n * inDirection;
367
		ioSuspensionLength = Clamp(oc.Dot(inDirection), 0.0f, wheel_settings->mSuspensionMaxLength);
368
	}
369
	else
370
	{
371
		// If the normal is pointing away we assume there's no collision anymore
372
		ioSuspensionLength = wheel_settings->mSuspensionMaxLength;
373
	}
374
}
375

376
JPH_NAMESPACE_END
377

378
Product

Resources

Company
