2
0

ContactListenerImpl.cpp 5.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187
  1. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  2. // SPDX-License-Identifier: MIT
  3. #include <TestFramework.h>
  4. #include <Utils/ContactListenerImpl.h>
  5. #include <Renderer/DebugRendererImp.h>
  6. #include <Physics/Body/Body.h>
  7. ValidateResult ContactListenerImpl::OnContactValidate(const Body &inBody1, const Body &inBody2, const CollideShapeResult &inCollisionResult)
  8. {
  9. // Expect body 1 to be dynamic
  10. if (!inBody1.IsDynamic())
  11. JPH_BREAKPOINT;
  12. ValidateResult result;
  13. if (mNext != nullptr)
  14. result = mNext->OnContactValidate(inBody1, inBody2, inCollisionResult);
  15. else
  16. result = ContactListener::OnContactValidate(inBody1, inBody2, inCollisionResult);
  17. Trace("Validate %d and %d result %d", inBody1.GetID().GetIndex(), inBody2.GetID().GetIndex(), (int)result);
  18. return result;
  19. }
  20. void ContactListenerImpl::OnContactAdded(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
  21. {
  22. // Expect bodies to be sorted
  23. if (!(inBody1.GetID() < inBody2.GetID()))
  24. JPH_BREAKPOINT;
  25. Trace("Contact added %d (%08x) and %d (%08x)", inBody1.GetID().GetIndex(), inManifold.mSubShapeID1.GetValue(), inBody2.GetID().GetIndex(), inManifold.mSubShapeID2.GetValue());
  26. DebugRenderer::sInstance->DrawWirePolygon(inManifold.mWorldSpaceContactPointsOn1, Color::sGreen, 0.05f);
  27. DebugRenderer::sInstance->DrawWirePolygon(inManifold.mWorldSpaceContactPointsOn2, Color::sGreen, 0.05f);
  28. DebugRenderer::sInstance->DrawArrow(inManifold.mWorldSpaceContactPointsOn1[0], inManifold.mWorldSpaceContactPointsOn1[0] + inManifold.mWorldSpaceNormal, Color::sGreen, 0.05f);
  29. // Insert new manifold into state map
  30. {
  31. lock_guard lock(mStateMutex);
  32. SubShapeIDPair key(inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2);
  33. if (mState.find(key) != mState.end())
  34. JPH_BREAKPOINT; // Added contact that already existed
  35. mState[key] = inManifold.mWorldSpaceContactPointsOn1;
  36. }
  37. if (mNext != nullptr)
  38. mNext->OnContactAdded(inBody1, inBody2, inManifold, ioSettings);
  39. }
  40. void ContactListenerImpl::OnContactPersisted(const Body &inBody1, const Body &inBody2, const ContactManifold &inManifold, ContactSettings &ioSettings)
  41. {
  42. // Expect bodies to be sorted
  43. if (!(inBody1.GetID() < inBody2.GetID()))
  44. JPH_BREAKPOINT;
  45. Trace("Contact persisted %d (%08x) and %d (%08x)", inBody1.GetID().GetIndex(), inManifold.mSubShapeID1.GetValue(), inBody2.GetID().GetIndex(), inManifold.mSubShapeID2.GetValue());
  46. DebugRenderer::sInstance->DrawWirePolygon(inManifold.mWorldSpaceContactPointsOn1, Color::sYellow, 0.05f);
  47. DebugRenderer::sInstance->DrawWirePolygon(inManifold.mWorldSpaceContactPointsOn2, Color::sYellow, 0.05f);
  48. DebugRenderer::sInstance->DrawArrow(inManifold.mWorldSpaceContactPointsOn1[0], inManifold.mWorldSpaceContactPointsOn1[0] + inManifold.mWorldSpaceNormal, Color::sYellow, 0.05f);
  49. // Update existing manifold in state map
  50. {
  51. lock_guard lock(mStateMutex);
  52. SubShapeIDPair key(inBody1.GetID(), inManifold.mSubShapeID1, inBody2.GetID(), inManifold.mSubShapeID2);
  53. StateMap::iterator i = mState.find(key);
  54. if (i != mState.end())
  55. i->second = inManifold.mWorldSpaceContactPointsOn1;
  56. else
  57. JPH_BREAKPOINT; // Persisted contact that didn't exist
  58. }
  59. if (mNext != nullptr)
  60. mNext->OnContactPersisted(inBody1, inBody2, inManifold, ioSettings);
  61. }
  62. void ContactListenerImpl::OnContactRemoved(const SubShapeIDPair &inSubShapePair)
  63. {
  64. // Expect bodies to be sorted
  65. if (!(inSubShapePair.GetBody1ID() < inSubShapePair.GetBody2ID()))
  66. JPH_BREAKPOINT;
  67. Trace("Contact removed %d (%08x) and %d (%08x)", inSubShapePair.GetBody1ID().GetIndex(), inSubShapePair.GetSubShapeID1().GetValue(), inSubShapePair.GetBody2ID().GetIndex(), inSubShapePair.GetSubShapeID2().GetValue());
  68. // Update existing manifold in state map
  69. {
  70. lock_guard lock(mStateMutex);
  71. StateMap::iterator i = mState.find(inSubShapePair);
  72. if (i != mState.end())
  73. mState.erase(i);
  74. else
  75. JPH_BREAKPOINT; // Removed contact that didn't exist
  76. }
  77. if (mNext != nullptr)
  78. mNext->OnContactRemoved(inSubShapePair);
  79. }
  80. void ContactListenerImpl::SaveState(StateRecorder &inStream) const
  81. {
  82. // Write length
  83. inStream.Write(mState.size());
  84. // Get and sort keys
  85. vector<SubShapeIDPair> keys;
  86. for (const StateMap::value_type &kv : mState)
  87. keys.push_back(kv.first);
  88. sort(keys.begin(), keys.end());
  89. // Write key value pairs
  90. for (const SubShapeIDPair &k : keys)
  91. {
  92. // Write key
  93. inStream.Write(k);
  94. // Write value
  95. const ContactPoints &cp = mState.find(k)->second;
  96. inStream.Write(cp.size());
  97. inStream.WriteBytes(cp.data(), cp.size() * sizeof(Vec3));
  98. }
  99. }
  100. void ContactListenerImpl::RestoreState(StateRecorder &inStream)
  101. {
  102. Trace("Restore Contact State");
  103. // Read length
  104. StateMap::size_type length;
  105. if (inStream.IsValidating())
  106. length = mState.size();
  107. inStream.Read(length);
  108. vector<SubShapeIDPair> keys;
  109. // Clear the state and remember the old state for validation
  110. StateMap old_state;
  111. old_state.swap(mState);
  112. // Prepopulate keys and values with current values if we're validating
  113. if (inStream.IsValidating())
  114. {
  115. // Get and sort keys
  116. for (const StateMap::value_type &kv : old_state)
  117. keys.push_back(kv.first);
  118. sort(keys.begin(), keys.end());
  119. }
  120. // Ensure we have the corect size
  121. keys.resize(length);
  122. for (size_t i = 0; i < length; ++i)
  123. {
  124. // Read key
  125. SubShapeIDPair key;
  126. if (inStream.IsValidating())
  127. key = keys[i];
  128. inStream.Read(key);
  129. // Read value length
  130. ContactPoints::size_type num_contacts;
  131. if (inStream.IsValidating())
  132. num_contacts = old_state[key].size();
  133. inStream.Read(num_contacts);
  134. // Read values
  135. ContactPoints contacts;
  136. if (inStream.IsValidating())
  137. contacts = old_state[key];
  138. contacts.resize(num_contacts);
  139. inStream.ReadBytes(contacts.data(), num_contacts * sizeof(Vec3));
  140. // Store the new value
  141. mState[key] = contacts;
  142. }
  143. }
  144. void ContactListenerImpl::DrawState()
  145. {
  146. Trace("Draw Contact State");
  147. lock_guard lock(mStateMutex);
  148. for (const StateMap::value_type &kv : mState)
  149. for (Vec3 v : kv.second)
  150. DebugRenderer::sInstance->DrawWireSphere(v, 0.05f, Color::sRed, 1);
  151. }