-
Notifications
You must be signed in to change notification settings - Fork 434
/
CharacterVirtual.cpp
1469 lines (1253 loc) · 58.9 KB
/
CharacterVirtual.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Character/CharacterVirtual.h>
#include <Jolt/Physics/Body/Body.h>
#include <Jolt/Physics/PhysicsSystem.h>
#include <Jolt/Physics/Collision/ShapeCast.h>
#include <Jolt/Physics/Collision/CollideShape.h>
#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
#include <Jolt/Core/QuickSort.h>
#include <Jolt/Geometry/ConvexSupport.h>
#include <Jolt/Geometry/GJKClosestPoint.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_BEGIN
CharacterVirtual::CharacterVirtual(const CharacterVirtualSettings *inSettings, RVec3Arg inPosition, QuatArg inRotation, PhysicsSystem *inSystem) :
CharacterBase(inSettings, inSystem),
mBackFaceMode(inSettings->mBackFaceMode),
mPredictiveContactDistance(inSettings->mPredictiveContactDistance),
mMaxCollisionIterations(inSettings->mMaxCollisionIterations),
mMaxConstraintIterations(inSettings->mMaxConstraintIterations),
mMinTimeRemaining(inSettings->mMinTimeRemaining),
mCollisionTolerance(inSettings->mCollisionTolerance),
mCharacterPadding(inSettings->mCharacterPadding),
mMaxNumHits(inSettings->mMaxNumHits),
mHitReductionCosMaxAngle(inSettings->mHitReductionCosMaxAngle),
mPenetrationRecoverySpeed(inSettings->mPenetrationRecoverySpeed),
mShapeOffset(inSettings->mShapeOffset),
mPosition(inPosition),
mRotation(inRotation)
{
// Copy settings
SetMaxStrength(inSettings->mMaxStrength);
SetMass(inSettings->mMass);
}
void CharacterVirtual::GetAdjustedBodyVelocity(const Body& inBody, Vec3 &outLinearVelocity, Vec3 &outAngularVelocity) const
{
// Get real velocity of body
if (!inBody.IsStatic())
{
const MotionProperties *mp = inBody.GetMotionPropertiesUnchecked();
outLinearVelocity = mp->GetLinearVelocity();
outAngularVelocity = mp->GetAngularVelocity();
}
else
{
outLinearVelocity = outAngularVelocity = Vec3::sZero();
}
// Allow application to override
if (mListener != nullptr)
mListener->OnAdjustBodyVelocity(this, inBody, outLinearVelocity, outAngularVelocity);
}
Vec3 CharacterVirtual::CalculateCharacterGroundVelocity(RVec3Arg inCenterOfMass, Vec3Arg inLinearVelocity, Vec3Arg inAngularVelocity, float inDeltaTime) const
{
// Get angular velocity
float angular_velocity_len_sq = inAngularVelocity.LengthSq();
if (angular_velocity_len_sq < 1.0e-12f)
return inLinearVelocity;
float angular_velocity_len = sqrt(angular_velocity_len_sq);
// Calculate the rotation that the object will make in the time step
Quat rotation = Quat::sRotation(inAngularVelocity / angular_velocity_len, angular_velocity_len * inDeltaTime);
// Calculate where the new character position will be
RVec3 new_position = inCenterOfMass + rotation * Vec3(mPosition - inCenterOfMass);
// Calculate the velocity
return inLinearVelocity + Vec3(new_position - mPosition) / inDeltaTime;
}
template <class taCollector>
void CharacterVirtual::sFillContactProperties(const CharacterVirtual *inCharacter, Contact &outContact, const Body &inBody, Vec3Arg inUp, RVec3Arg inBaseOffset, const taCollector &inCollector, const CollideShapeResult &inResult)
{
// Get adjusted body velocity
Vec3 linear_velocity, angular_velocity;
inCharacter->GetAdjustedBodyVelocity(inBody, linear_velocity, angular_velocity);
outContact.mPosition = inBaseOffset + inResult.mContactPointOn2;
outContact.mLinearVelocity = linear_velocity + angular_velocity.Cross(Vec3(outContact.mPosition - inBody.GetCenterOfMassPosition())); // Calculate point velocity
outContact.mContactNormal = -inResult.mPenetrationAxis.NormalizedOr(Vec3::sZero());
outContact.mSurfaceNormal = inCollector.GetContext()->GetWorldSpaceSurfaceNormal(inResult.mSubShapeID2, outContact.mPosition);
if (outContact.mContactNormal.Dot(outContact.mSurfaceNormal) < 0.0f)
outContact.mSurfaceNormal = -outContact.mSurfaceNormal; // Flip surface normal if we're hitting a back face
if (outContact.mContactNormal.Dot(inUp) > outContact.mSurfaceNormal.Dot(inUp))
outContact.mSurfaceNormal = outContact.mContactNormal; // Replace surface normal with contact normal if the contact normal is pointing more upwards
outContact.mDistance = -inResult.mPenetrationDepth;
outContact.mBodyB = inResult.mBodyID2;
outContact.mSubShapeIDB = inResult.mSubShapeID2;
outContact.mMotionTypeB = inBody.GetMotionType();
outContact.mUserData = inBody.GetUserData();
outContact.mMaterial = inCollector.GetContext()->GetMaterial(inResult.mSubShapeID2);
}
void CharacterVirtual::ContactCollector::AddHit(const CollideShapeResult &inResult)
{
// If we exceed our contact limit, try to clean up near-duplicate contacts
if (mContacts.size() == mMaxHits)
{
// Flag that we hit this code path
mMaxHitsExceeded = true;
// Check if we can do reduction
if (mHitReductionCosMaxAngle > -1.0f)
{
// Loop all contacts and find similar contacts
for (int i = (int)mContacts.size() - 1; i >= 0; --i)
{
Contact &contact_i = mContacts[i];
for (int j = i - 1; j >= 0; --j)
{
Contact &contact_j = mContacts[j];
if (contact_i.mBodyB == contact_j.mBodyB // Same body
&& contact_i.mContactNormal.Dot(contact_j.mContactNormal) > mHitReductionCosMaxAngle) // Very similar contact normals
{
// Remove the contact with the biggest distance
bool i_is_last = i == (int)mContacts.size() - 1;
if (contact_i.mDistance > contact_j.mDistance)
{
// Remove i
if (!i_is_last)
contact_i = mContacts.back();
mContacts.pop_back();
// Break out of the loop, i is now an element that we already processed
break;
}
else
{
// Remove j
contact_j = mContacts.back();
mContacts.pop_back();
// If i was the last element, we just moved it into position j. Break out of the loop, we'll see it again later.
if (i_is_last)
break;
}
}
}
}
}
if (mContacts.size() == mMaxHits)
{
// There are still too many hits, give up!
ForceEarlyOut();
return;
}
}
BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2);
if (lock.SucceededAndIsInBroadPhase())
{
// We don't collide with sensors, note that you should set up your collision layers so that sensors don't collide with the character.
// Rejecting the contact here means a lot of extra work for the collision detection system.
const Body &body = lock.GetBody();
if (!body.IsSensor())
{
mContacts.emplace_back();
Contact &contact = mContacts.back();
sFillContactProperties(mCharacter, contact, body, mUp, mBaseOffset, *this, inResult);
contact.mFraction = 0.0f;
}
}
}
void CharacterVirtual::ContactCastCollector::AddHit(const ShapeCastResult &inResult)
{
// Should not have gotten here without a lower fraction
JPH_ASSERT(inResult.mFraction < mContact.mFraction);
if (inResult.mFraction > 0.0f // Ignore collisions at fraction = 0
&& inResult.mPenetrationAxis.Dot(mDisplacement) > 0.0f) // Ignore penetrations that we're moving away from
{
// Test if this contact should be ignored
for (const IgnoredContact &c : mIgnoredContacts)
if (c.mBodyID == inResult.mBodyID2 && c.mSubShapeID == inResult.mSubShapeID2)
return;
Contact contact;
// Lock body only while we fetch contact properties
{
BodyLockRead lock(mSystem->GetBodyLockInterface(), inResult.mBodyID2);
if (!lock.SucceededAndIsInBroadPhase())
return;
// We don't collide with sensors, note that you should set up your collision layers so that sensors don't collide with the character.
// Rejecting the contact here means a lot of extra work for the collision detection system.
const Body &body = lock.GetBody();
if (body.IsSensor())
return;
// Convert the hit result into a contact
sFillContactProperties(mCharacter, contact, body, mUp, mBaseOffset, *this, inResult);
}
contact.mFraction = inResult.mFraction;
// Check if the contact that will make us penetrate more than the allowed tolerance
if (contact.mDistance + contact.mContactNormal.Dot(mDisplacement) < -mCharacter->mCollisionTolerance
&& mCharacter->ValidateContact(contact))
{
mContact = contact;
UpdateEarlyOutFraction(contact.mFraction);
}
}
}
void CharacterVirtual::CheckCollision(RVec3Arg inPosition, QuatArg inRotation, Vec3Arg inMovementDirection, float inMaxSeparationDistance, const Shape *inShape, RVec3Arg inBaseOffset, CollideShapeCollector &ioCollector, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
{
// Query shape transform
RMat44 transform = GetCenterOfMassTransform(inPosition, inRotation, inShape);
// Settings for collide shape
CollideShapeSettings settings;
settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive;
settings.mBackFaceMode = mBackFaceMode;
settings.mActiveEdgeMovementDirection = inMovementDirection;
settings.mMaxSeparationDistance = mCharacterPadding + inMaxSeparationDistance;
// Collide shape
mSystem->GetNarrowPhaseQuery().CollideShape(inShape, Vec3::sReplicate(1.0f), transform, settings, inBaseOffset, ioCollector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
}
void CharacterVirtual::GetContactsAtPosition(RVec3Arg inPosition, Vec3Arg inMovementDirection, const Shape *inShape, TempContactList &outContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
{
// Remove previous results
outContacts.clear();
// Collide shape
ContactCollector collector(mSystem, this, mMaxNumHits, mHitReductionCosMaxAngle, mUp, mPosition, outContacts);
CheckCollision(inPosition, mRotation, inMovementDirection, mPredictiveContactDistance, inShape, mPosition, collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
// Flag if we exceeded the max number of hits
mMaxHitsExceeded = collector.mMaxHitsExceeded;
// Reduce distance to contact by padding to ensure we stay away from the object by a little margin
// (this will make collision detection cheaper - especially for sweep tests as they won't hit the surface if we're properly sliding)
for (Contact &c : outContacts)
c.mDistance -= mCharacterPadding;
}
void CharacterVirtual::RemoveConflictingContacts(TempContactList &ioContacts, IgnoredContactList &outIgnoredContacts) const
{
// Only use this algorithm if we're penetrating further than this (due to numerical precision issues we can always penetrate a little bit and we don't want to discard contacts if they just have a tiny penetration)
// We do need to account for padding (see GetContactsAtPosition) that is removed from the contact distances, to compensate we add it to the cMinRequiredPenetration
const float cMinRequiredPenetration = 1.25f * mCharacterPadding;
// Discard conflicting penetrating contacts
for (size_t c1 = 0; c1 < ioContacts.size(); c1++)
{
Contact &contact1 = ioContacts[c1];
if (contact1.mDistance <= -cMinRequiredPenetration) // Only for penetrations
for (size_t c2 = c1 + 1; c2 < ioContacts.size(); c2++)
{
Contact &contact2 = ioContacts[c2];
if (contact1.mBodyB == contact2.mBodyB // Only same body
&& contact2.mDistance <= -cMinRequiredPenetration // Only for penetrations
&& contact1.mContactNormal.Dot(contact2.mContactNormal) < 0.0f) // Only opposing normals
{
// Discard contacts with the least amount of penetration
if (contact1.mDistance < contact2.mDistance)
{
// Discard the 2nd contact
outIgnoredContacts.emplace_back(contact2.mBodyB, contact2.mSubShapeIDB);
ioContacts.erase(ioContacts.begin() + c2);
c2--;
}
else
{
// Discard the first contact
outIgnoredContacts.emplace_back(contact1.mBodyB, contact1.mSubShapeIDB);
ioContacts.erase(ioContacts.begin() + c1);
c1--;
break;
}
}
}
}
}
bool CharacterVirtual::ValidateContact(const Contact &inContact) const
{
if (mListener == nullptr)
return true;
return mListener->OnContactValidate(this, inContact.mBodyB, inContact.mSubShapeIDB);
}
template <class T>
inline static bool sCorrectFractionForCharacterPadding(const Shape *inShape, Mat44Arg inStart, Vec3Arg inDisplacement, const T &inPolygon, float &ioFraction)
{
if (inShape->GetType() == EShapeType::Convex)
{
// Get the support function for the shape we're casting
const ConvexShape *convex_shape = static_cast<const ConvexShape *>(inShape);
ConvexShape::SupportBuffer buffer;
const ConvexShape::Support *support = convex_shape->GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sReplicate(1.0f));
// Cast the shape against the polygon
GJKClosestPoint gjk;
return gjk.CastShape(inStart, inDisplacement, cDefaultCollisionTolerance, *support, inPolygon, ioFraction);
}
else if (inShape->GetSubType() == EShapeSubType::RotatedTranslated)
{
const RotatedTranslatedShape *rt_shape = static_cast<const RotatedTranslatedShape *>(inShape);
return sCorrectFractionForCharacterPadding(rt_shape->GetInnerShape(), inStart * Mat44::sRotation(rt_shape->GetRotation()), inDisplacement, inPolygon, ioFraction);
}
else
{
JPH_ASSERT(false, "Not supported yet!");
return false;
}
}
bool CharacterVirtual::GetFirstContactForSweep(RVec3Arg inPosition, Vec3Arg inDisplacement, Contact &outContact, const IgnoredContactList &inIgnoredContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter) const
{
// Too small distance -> skip checking
float displacement_len_sq = inDisplacement.LengthSq();
if (displacement_len_sq < 1.0e-8f)
return false;
// Calculate start transform
RMat44 start = GetCenterOfMassTransform(inPosition, mRotation, mShape);
// Settings for the cast
ShapeCastSettings settings;
settings.mBackFaceModeTriangles = mBackFaceMode;
settings.mBackFaceModeConvex = EBackFaceMode::IgnoreBackFaces;
settings.mActiveEdgeMode = EActiveEdgeMode::CollideOnlyWithActive;
settings.mUseShrunkenShapeAndConvexRadius = true;
settings.mReturnDeepestPoint = false;
// Cast shape
Contact contact;
contact.mFraction = 1.0f + FLT_EPSILON;
ContactCastCollector collector(mSystem, this, inDisplacement, mUp, inIgnoredContacts, start.GetTranslation(), contact);
RShapeCast shape_cast(mShape, Vec3::sReplicate(1.0f), start, inDisplacement);
mSystem->GetNarrowPhaseQuery().CastShape(shape_cast, settings, start.GetTranslation(), collector, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
if (contact.mBodyB.IsInvalid())
return false;
// Store contact
outContact = contact;
// Fetch the face we're colliding with
TransformedShape ts = mSystem->GetBodyInterface().GetTransformedShape(outContact.mBodyB);
Shape::SupportingFace face;
ts.GetSupportingFace(outContact.mSubShapeIDB, -outContact.mContactNormal, start.GetTranslation(), face);
bool corrected = false;
if (face.size() >= 2)
{
// Inflate the colliding face by the character padding
PolygonConvexSupport polygon(face);
AddConvexRadius add_cvx(polygon, mCharacterPadding);
// Correct fraction to hit this inflated face instead of the inner shape
corrected = sCorrectFractionForCharacterPadding(mShape, start.GetRotation(), inDisplacement, add_cvx, outContact.mFraction);
}
if (!corrected)
{
// When there's only a single contact point or when we were unable to correct the fraction,
// we can just move the fraction back so that the character and its padding don't hit the contact point anymore
outContact.mFraction = max(0.0f, outContact.mFraction - mCharacterPadding / sqrt(displacement_len_sq));
}
return true;
}
void CharacterVirtual::DetermineConstraints(TempContactList &inContacts, float inDeltaTime, ConstraintList &outConstraints) const
{
for (Contact &c : inContacts)
{
Vec3 contact_velocity = c.mLinearVelocity;
// Penetrating contact: Add a contact velocity that pushes the character out at the desired speed
if (c.mDistance < 0.0f)
contact_velocity -= c.mContactNormal * c.mDistance * mPenetrationRecoverySpeed / inDeltaTime;
// Convert to a constraint
outConstraints.emplace_back();
Constraint &constraint = outConstraints.back();
constraint.mContact = &c;
constraint.mLinearVelocity = contact_velocity;
constraint.mPlane = Plane(c.mContactNormal, c.mDistance);
// Next check if the angle is too steep and if it is add an additional constraint that holds the character back
if (IsSlopeTooSteep(c.mSurfaceNormal))
{
// Only take planes that point up.
// Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement.
float dot = c.mContactNormal.Dot(mUp);
if (dot > 1.0e-3f) // Add a little slack, if the normal is perfectly horizontal we already have our vertical plane.
{
// Make horizontal normal
Vec3 normal = (c.mContactNormal - dot * mUp).Normalized();
// Create a secondary constraint that blocks horizontal movement
outConstraints.emplace_back();
Constraint &vertical_constraint = outConstraints.back();
vertical_constraint.mContact = &c;
vertical_constraint.mLinearVelocity = contact_velocity.Dot(normal) * normal; // Project the contact velocity on the new normal so that both planes push at an equal rate
vertical_constraint.mPlane = Plane(normal, c.mDistance / normal.Dot(c.mContactNormal)); // Calculate the distance we have to travel horizontally to hit the contact plane
}
}
}
}
bool CharacterVirtual::HandleContact(Vec3Arg inVelocity, Constraint &ioConstraint, float inDeltaTime) const
{
Contact &contact = *ioConstraint.mContact;
// Validate the contact point
if (!ValidateContact(contact))
return false;
// Send contact added event
CharacterContactSettings settings;
if (mListener != nullptr)
mListener->OnContactAdded(this, contact.mBodyB, contact.mSubShapeIDB, contact.mPosition, -contact.mContactNormal, settings);
contact.mCanPushCharacter = settings.mCanPushCharacter;
// If body B cannot receive an impulse, we're done
if (!settings.mCanReceiveImpulses || contact.mMotionTypeB != EMotionType::Dynamic)
return true;
// Lock the body we're colliding with
BodyLockWrite lock(mSystem->GetBodyLockInterface(), contact.mBodyB);
if (!lock.SucceededAndIsInBroadPhase())
return false; // Body has been removed, we should not collide with it anymore
const Body &body = lock.GetBody();
// Calculate the velocity that we want to apply at B so that it will start moving at the character's speed at the contact point
constexpr float cDamping = 0.9f;
constexpr float cPenetrationResolution = 0.4f;
Vec3 relative_velocity = inVelocity - contact.mLinearVelocity;
float projected_velocity = relative_velocity.Dot(contact.mContactNormal);
float delta_velocity = -projected_velocity * cDamping - min(contact.mDistance, 0.0f) * cPenetrationResolution / inDeltaTime;
// Don't apply impulses if we're separating
if (delta_velocity < 0.0f)
return true;
// Determine mass properties of the body we're colliding with
const MotionProperties *motion_properties = body.GetMotionProperties();
RVec3 center_of_mass = body.GetCenterOfMassPosition();
Mat44 inverse_inertia = body.GetInverseInertia();
float inverse_mass = motion_properties->GetInverseMass();
// Calculate the inverse of the mass of body B as seen at the contact point in the direction of the contact normal
Vec3 jacobian = Vec3(contact.mPosition - center_of_mass).Cross(contact.mContactNormal);
float inv_effective_mass = inverse_inertia.Multiply3x3(jacobian).Dot(jacobian) + inverse_mass;
// Impulse P = M dv
float impulse = delta_velocity / inv_effective_mass;
// Clamp the impulse according to the character strength, character strength is a force in newtons, P = F dt
float max_impulse = mMaxStrength * inDeltaTime;
impulse = min(impulse, max_impulse);
// Calculate the world space impulse to apply
Vec3 world_impulse = -impulse * contact.mContactNormal;
// Cancel impulse in down direction (we apply gravity later)
float impulse_dot_up = world_impulse.Dot(mUp);
if (impulse_dot_up < 0.0f)
world_impulse -= impulse_dot_up * mUp;
// Now apply the impulse (body is already locked so we use the no-lock interface)
mSystem->GetBodyInterfaceNoLock().AddImpulse(contact.mBodyB, world_impulse, contact.mPosition);
return true;
}
void CharacterVirtual::SolveConstraints(Vec3Arg inVelocity, float inDeltaTime, float inTimeRemaining, ConstraintList &ioConstraints, IgnoredContactList &ioIgnoredContacts, float &outTimeSimulated, Vec3 &outDisplacement, TempAllocator &inAllocator
#ifdef JPH_DEBUG_RENDERER
, bool inDrawConstraints
#endif // JPH_DEBUG_RENDERER
) const
{
// If there are no constraints we can immediately move to our target
if (ioConstraints.empty())
{
outDisplacement = inVelocity * inTimeRemaining;
outTimeSimulated = inTimeRemaining;
return;
}
// Create array that holds the constraints in order of time of impact (sort will happen later)
std::vector<Constraint *, STLTempAllocator<Constraint *>> sorted_constraints(inAllocator);
sorted_constraints.resize(ioConstraints.size());
for (size_t index = 0; index < sorted_constraints.size(); index++)
sorted_constraints[index] = &ioConstraints[index];
// This is the velocity we use for the displacement, if we hit something it will be shortened
Vec3 velocity = inVelocity;
// Keep track of the last velocity that was applied to the character so that we can detect when the velocity reverses
Vec3 last_velocity = inVelocity;
// Start with no displacement
outDisplacement = Vec3::sZero();
outTimeSimulated = 0.0f;
// These are the contacts that we hit previously without moving a significant distance
std::vector<Constraint *, STLTempAllocator<Constraint *>> previous_contacts(inAllocator);
previous_contacts.resize(mMaxConstraintIterations);
int num_previous_contacts = 0;
// Loop for a max amount of iterations
for (uint iteration = 0; iteration < mMaxConstraintIterations; iteration++)
{
// Calculate time of impact for all constraints
for (Constraint &c : ioConstraints)
{
// Project velocity on plane direction
c.mProjectedVelocity = c.mPlane.GetNormal().Dot(c.mLinearVelocity - velocity);
if (c.mProjectedVelocity < 1.0e-6f)
{
c.mTOI = FLT_MAX;
}
else
{
// Distance to plane
float dist = c.mPlane.SignedDistance(outDisplacement);
if (dist - c.mProjectedVelocity * inTimeRemaining > -1.0e-4f)
{
// Too little penetration, accept the movement
c.mTOI = FLT_MAX;
}
else
{
// Calculate time of impact
c.mTOI = max(0.0f, dist / c.mProjectedVelocity);
}
}
}
// Sort constraints on proximity
QuickSort(sorted_constraints.begin(), sorted_constraints.end(), [](const Constraint *inLHS, const Constraint *inRHS) {
// If both constraints hit at t = 0 then order the one that will push the character furthest first
// Note that because we add velocity to penetrating contacts, this will also resolve contacts that penetrate the most
if (inLHS->mTOI <= 0.0f && inRHS->mTOI <= 0.0f)
return inLHS->mProjectedVelocity > inRHS->mProjectedVelocity;
// Then sort on time of impact
if (inLHS->mTOI != inRHS->mTOI)
return inLHS->mTOI < inRHS->mTOI;
// As a tie breaker sort static first so it has the most influence
return inLHS->mContact->mMotionTypeB > inRHS->mContact->mMotionTypeB;
});
// Find the first valid constraint
Constraint *constraint = nullptr;
for (Constraint *c : sorted_constraints)
{
// Take the first contact and see if we can reach it
if (c->mTOI >= inTimeRemaining)
{
// We can reach our goal!
outDisplacement += velocity * inTimeRemaining;
outTimeSimulated += inTimeRemaining;
return;
}
// Test if this contact was discarded by the contact callback before
if (c->mContact->mWasDiscarded)
continue;
// Check if we made contact with this before
if (!c->mContact->mHadCollision)
{
// Handle the contact
if (!HandleContact(velocity, *c, inDeltaTime))
{
// Constraint should be ignored, remove it from the list
c->mContact->mWasDiscarded = true;
// Mark it as ignored for GetFirstContactForSweep
ioIgnoredContacts.emplace_back(c->mContact->mBodyB, c->mContact->mSubShapeIDB);
continue;
}
c->mContact->mHadCollision = true;
}
// Cancel velocity of constraint if it cannot push the character
if (!c->mContact->mCanPushCharacter)
c->mLinearVelocity = Vec3::sZero();
// We found the first constraint that we want to collide with
constraint = c;
break;
}
if (constraint == nullptr)
{
// All constraints were discarded, we can reach our goal!
outDisplacement += velocity * inTimeRemaining;
outTimeSimulated += inTimeRemaining;
return;
}
// Move to the contact
outDisplacement += velocity * constraint->mTOI;
inTimeRemaining -= constraint->mTOI;
outTimeSimulated += constraint->mTOI;
// If there's not enough time left to be simulated, bail
if (inTimeRemaining < mMinTimeRemaining)
return;
// If we've moved significantly, clear all previous contacts
if (constraint->mTOI > 1.0e-4f)
num_previous_contacts = 0;
// Get the normal of the plane we're hitting
Vec3 plane_normal = constraint->mPlane.GetNormal();
// Get the relative velocity between the character and the constraint
Vec3 relative_velocity = velocity - constraint->mLinearVelocity;
// Calculate new velocity if we cancel the relative velocity in the normal direction
Vec3 new_velocity = velocity - relative_velocity.Dot(plane_normal) * plane_normal;
// Find the normal of the previous contact that we will violate the most if we move in this new direction
float highest_penetration = 0.0f;
Constraint *other_constraint = nullptr;
for (Constraint **c = previous_contacts.data(); c < previous_contacts.data() + num_previous_contacts; ++c)
if (*c != constraint)
{
// Calculate how much we will penetrate if we move in this direction
Vec3 other_normal = (*c)->mPlane.GetNormal();
float penetration = ((*c)->mLinearVelocity - new_velocity).Dot(other_normal);
if (penetration > highest_penetration)
{
// We don't want parallel or anti-parallel normals as that will cause our cross product below to become zero. Slack is approx 10 degrees.
float dot = other_normal.Dot(plane_normal);
if (dot < 0.984f && dot > -0.984f)
{
highest_penetration = penetration;
other_constraint = *c;
}
}
}
// Check if we found a 2nd constraint
if (other_constraint != nullptr)
{
// Calculate the sliding direction and project the new velocity onto that sliding direction
Vec3 other_normal = other_constraint->mPlane.GetNormal();
Vec3 slide_dir = plane_normal.Cross(other_normal).Normalized();
Vec3 velocity_in_slide_dir = new_velocity.Dot(slide_dir) * slide_dir;
// Cancel the constraint velocity in the other constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes
constraint->mLinearVelocity -= min(0.0f, constraint->mLinearVelocity.Dot(other_normal)) * other_normal;
// Cancel the other constraints velocity in this constraint plane's direction so that we won't try to apply it again and keep ping ponging between planes
other_constraint->mLinearVelocity -= min(0.0f, other_constraint->mLinearVelocity.Dot(plane_normal)) * plane_normal;
// Calculate the velocity of this constraint perpendicular to the slide direction
Vec3 perpendicular_velocity = constraint->mLinearVelocity - constraint->mLinearVelocity.Dot(slide_dir) * slide_dir;
// Calculate the velocity of the other constraint perpendicular to the slide direction
Vec3 other_perpendicular_velocity = other_constraint->mLinearVelocity - other_constraint->mLinearVelocity.Dot(slide_dir) * slide_dir;
// Add all components together
new_velocity = velocity_in_slide_dir + perpendicular_velocity + other_perpendicular_velocity;
}
// Allow application to modify calculated velocity
if (mListener != nullptr)
mListener->OnContactSolve(this, constraint->mContact->mBodyB, constraint->mContact->mSubShapeIDB, constraint->mContact->mPosition, constraint->mContact->mContactNormal, constraint->mContact->mLinearVelocity, constraint->mContact->mMaterial, velocity, new_velocity);
#ifdef JPH_DEBUG_RENDERER
if (inDrawConstraints)
{
// Calculate where to draw
RVec3 offset = mPosition + Vec3(0, 0, 2.5f * (iteration + 1));
// Draw constraint plane
DebugRenderer::sInstance->DrawPlane(offset, constraint->mPlane.GetNormal(), Color::sCyan, 1.0f);
// Draw 2nd constraint plane
if (other_constraint != nullptr)
DebugRenderer::sInstance->DrawPlane(offset, other_constraint->mPlane.GetNormal(), Color::sBlue, 1.0f);
// Draw starting velocity
DebugRenderer::sInstance->DrawArrow(offset, offset + velocity, Color::sGreen, 0.05f);
// Draw resulting velocity
DebugRenderer::sInstance->DrawArrow(offset, offset + new_velocity, Color::sRed, 0.05f);
}
#endif // JPH_DEBUG_RENDERER
// Update the velocity
velocity = new_velocity;
// Add the contact to the list so that next iteration we can avoid violating it again
previous_contacts[num_previous_contacts] = constraint;
num_previous_contacts++;
// Check early out
if (constraint->mProjectedVelocity < 1.0e-8f // Constraint should not be pushing, otherwise there may be other constraints that are pushing us
&& velocity.LengthSq() < 1.0e-8f) // There's not enough velocity left
return;
// If the constraint has velocity we accept the new velocity, otherwise check that we didn't reverse velocity
if (!constraint->mLinearVelocity.IsNearZero(1.0e-8f))
last_velocity = constraint->mLinearVelocity;
else if (velocity.Dot(last_velocity) < 0.0f)
return;
}
}
void CharacterVirtual::UpdateSupportingContact(bool inSkipContactVelocityCheck, TempAllocator &inAllocator)
{
// Flag contacts as having a collision if they're close enough but ignore contacts we're moving away from.
// Note that if we did MoveShape before we want to preserve any contacts that it marked as colliding
for (Contact &c : mActiveContacts)
if (!c.mWasDiscarded
&& !c.mHadCollision
&& c.mDistance < mCollisionTolerance
&& (inSkipContactVelocityCheck || c.mSurfaceNormal.Dot(mLinearVelocity - c.mLinearVelocity) <= 1.0e-4f))
{
if (ValidateContact(c))
c.mHadCollision = true;
else
c.mWasDiscarded = true;
}
// Calculate transform that takes us to character local space
RMat44 inv_transform = RMat44::sInverseRotationTranslation(mRotation, mPosition);
// Determine if we're supported or not
int num_supported = 0;
int num_sliding = 0;
int num_avg_normal = 0;
Vec3 avg_normal = Vec3::sZero();
Vec3 avg_velocity = Vec3::sZero();
const Contact *supporting_contact = nullptr;
float max_cos_angle = -FLT_MAX;
const Contact *deepest_contact = nullptr;
float smallest_distance = FLT_MAX;
for (const Contact &c : mActiveContacts)
if (c.mHadCollision)
{
// Calculate the angle between the plane normal and the up direction
float cos_angle = c.mSurfaceNormal.Dot(mUp);
// Find the deepest contact
if (c.mDistance < smallest_distance)
{
deepest_contact = &c;
smallest_distance = c.mDistance;
}
// If this contact is in front of our plane, we cannot be supported by it
if (mSupportingVolume.SignedDistance(Vec3(inv_transform * c.mPosition)) > 0.0f)
continue;
// Find the contact with the normal that is pointing most upwards and store it
if (max_cos_angle < cos_angle)
{
supporting_contact = &c;
max_cos_angle = cos_angle;
}
// Check if this is a sliding or supported contact
bool is_supported = mCosMaxSlopeAngle > cNoMaxSlopeAngle || cos_angle >= mCosMaxSlopeAngle;
if (is_supported)
num_supported++;
else
num_sliding++;
// If the angle between the two is less than 85 degrees we also use it to calculate the average normal
if (cos_angle >= 0.08f)
{
avg_normal += c.mSurfaceNormal;
num_avg_normal++;
// For static or dynamic objects or for contacts that don't support us just take the contact velocity
if (c.mMotionTypeB != EMotionType::Kinematic || !is_supported)
avg_velocity += c.mLinearVelocity;
else
{
// For keyframed objects that support us calculate the velocity at our position rather than at the contact position so that we properly follow the object
BodyLockRead lock(mSystem->GetBodyLockInterface(), c.mBodyB);
if (lock.SucceededAndIsInBroadPhase())
{
const Body &body = lock.GetBody();
// Get adjusted body velocity
Vec3 linear_velocity, angular_velocity;
GetAdjustedBodyVelocity(body, linear_velocity, angular_velocity);
// Calculate the ground velocity
avg_velocity += CalculateCharacterGroundVelocity(body.GetCenterOfMassPosition(), linear_velocity, angular_velocity, mLastDeltaTime);
}
else
{
// Fall back to contact velocity
avg_velocity += c.mLinearVelocity;
}
}
}
}
// Take either the most supporting contact or the deepest contact
const Contact *best_contact = supporting_contact != nullptr? supporting_contact : deepest_contact;
// Calculate average normal and velocity
if (num_avg_normal >= 1)
{
mGroundNormal = avg_normal.Normalized();
mGroundVelocity = avg_velocity / float(num_avg_normal);
}
else if (best_contact != nullptr)
{
mGroundNormal = best_contact->mSurfaceNormal;
mGroundVelocity = best_contact->mLinearVelocity;
}
else
{
mGroundNormal = Vec3::sZero();
mGroundVelocity = Vec3::sZero();
}
// Copy contact properties
if (best_contact != nullptr)
{
mGroundBodyID = best_contact->mBodyB;
mGroundBodySubShapeID = best_contact->mSubShapeIDB;
mGroundPosition = best_contact->mPosition;
mGroundMaterial = best_contact->mMaterial;
mGroundUserData = best_contact->mUserData;
}
else
{
mGroundBodyID = BodyID();
mGroundBodySubShapeID = SubShapeID();
mGroundPosition = RVec3::sZero();
mGroundMaterial = PhysicsMaterial::sDefault;
mGroundUserData = 0;
}
// Determine ground state
if (num_supported > 0)
{
// We made contact with something that supports us
mGroundState = EGroundState::OnGround;
}
else if (num_sliding > 0)
{
// If we're sliding we may actually be standing on multiple sliding contacts in such a way that we can't slide off, in this case we're also supported
// Convert the contacts into constraints
TempContactList contacts(mActiveContacts.begin(), mActiveContacts.end(), inAllocator);
ConstraintList constraints(inAllocator);
constraints.reserve(contacts.size() * 2);
DetermineConstraints(contacts, mLastDeltaTime, constraints);
// Solve the displacement using these constraints, this is used to check if we didn't move at all because we are supported
Vec3 displacement;
float time_simulated;
IgnoredContactList ignored_contacts(inAllocator);
ignored_contacts.reserve(contacts.size());
SolveConstraints(-mUp, 1.0f, 1.0f, constraints, ignored_contacts, time_simulated, displacement, inAllocator);
// If we're blocked then we're supported, otherwise we're sliding
float min_required_displacement_sq = Square(0.6f * mLastDeltaTime);
if (time_simulated < 0.001f || displacement.LengthSq() < min_required_displacement_sq)
mGroundState = EGroundState::OnGround;
else
mGroundState = EGroundState::OnSteepGround;
}
else
{
// Not supported by anything
mGroundState = best_contact != nullptr? EGroundState::NotSupported : EGroundState::InAir;
}
}
void CharacterVirtual::StoreActiveContacts(const TempContactList &inContacts, TempAllocator &inAllocator)
{
mActiveContacts.assign(inContacts.begin(), inContacts.end());
UpdateSupportingContact(true, inAllocator);
}
void CharacterVirtual::MoveShape(RVec3 &ioPosition, Vec3Arg inVelocity, float inDeltaTime, ContactList *outActiveContacts, const BroadPhaseLayerFilter &inBroadPhaseLayerFilter, const ObjectLayerFilter &inObjectLayerFilter, const BodyFilter &inBodyFilter, const ShapeFilter &inShapeFilter, TempAllocator &inAllocator
#ifdef JPH_DEBUG_RENDERER
, bool inDrawConstraints
#endif // JPH_DEBUG_RENDERER
) const
{
Vec3 movement_direction = inVelocity.NormalizedOr(Vec3::sZero());
float time_remaining = inDeltaTime;
for (uint iteration = 0; iteration < mMaxCollisionIterations && time_remaining >= mMinTimeRemaining; iteration++)
{
// Determine contacts in the neighborhood
TempContactList contacts(inAllocator);
contacts.reserve(mMaxNumHits);
GetContactsAtPosition(ioPosition, movement_direction, mShape, contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter);
// Remove contacts with the same body that have conflicting normals
IgnoredContactList ignored_contacts(inAllocator);
ignored_contacts.reserve(contacts.size());
RemoveConflictingContacts(contacts, ignored_contacts);
// Convert contacts into constraints
ConstraintList constraints(inAllocator);
constraints.reserve(contacts.size() * 2);
DetermineConstraints(contacts, inDeltaTime, constraints);
#ifdef JPH_DEBUG_RENDERER
bool draw_constraints = inDrawConstraints && iteration == 0;
if (draw_constraints)
{
for (const Constraint &c : constraints)
{
// Draw contact point
DebugRenderer::sInstance->DrawMarker(c.mContact->mPosition, Color::sYellow, 0.05f);
Vec3 dist_to_plane = -c.mPlane.GetConstant() * c.mPlane.GetNormal();
// Draw arrow towards surface that we're hitting
DebugRenderer::sInstance->DrawArrow(c.mContact->mPosition, c.mContact->mPosition - dist_to_plane, Color::sYellow, 0.05f);
// Draw plane around the player position indicating the space that we can move
DebugRenderer::sInstance->DrawPlane(mPosition + dist_to_plane, c.mPlane.GetNormal(), Color::sCyan, 1.0f);
DebugRenderer::sInstance->DrawArrow(mPosition + dist_to_plane, mPosition + dist_to_plane + c.mContact->mSurfaceNormal, Color::sRed, 0.05f);
}
}
#endif // JPH_DEBUG_RENDERER
// Solve the displacement using these constraints
Vec3 displacement;
float time_simulated;
SolveConstraints(inVelocity, inDeltaTime, time_remaining, constraints, ignored_contacts, time_simulated, displacement, inAllocator
#ifdef JPH_DEBUG_RENDERER
, draw_constraints
#endif // JPH_DEBUG_RENDERER
);
// Store the contacts now that the colliding ones have been marked
if (outActiveContacts != nullptr)
outActiveContacts->assign(contacts.begin(), contacts.end());
// Do a sweep to test if the path is really unobstructed
Contact cast_contact;
if (GetFirstContactForSweep(ioPosition, displacement, cast_contact, ignored_contacts, inBroadPhaseLayerFilter, inObjectLayerFilter, inBodyFilter, inShapeFilter))
{
displacement *= cast_contact.mFraction;
time_simulated *= cast_contact.mFraction;
}
// Update the position
ioPosition += displacement;
time_remaining -= time_simulated;
// If the displacement during this iteration was too small we assume we cannot further progress this update
if (displacement.LengthSq() < 1.0e-8f)
break;
}
}
Vec3 CharacterVirtual::CancelVelocityTowardsSteepSlopes(Vec3Arg inDesiredVelocity) const
{
// If we're not pushing against a steep slope, return the desired velocity
// Note: This is important as WalkStairs overrides the ground state to OnGround when its first check fails but the second succeeds
if (mGroundState == CharacterVirtual::EGroundState::OnGround
|| mGroundState == CharacterVirtual::EGroundState::InAir)
return inDesiredVelocity;
Vec3 desired_velocity = inDesiredVelocity;
for (const Contact &c : mActiveContacts)
if (c.mHadCollision
&& IsSlopeTooSteep(c.mSurfaceNormal))
{
// Note that we use the contact normal to allow for better sliding as the surface normal may be in the opposite direction of movement.
Vec3 normal = c.mContactNormal;
// Remove normal vertical component
normal -= normal.Dot(mUp) * mUp;
// Cancel horizontal movement in opposite direction
float dot = normal.Dot(desired_velocity);
if (dot < 0.0f)