CompoundShape.cpp 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384
  1. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  2. // SPDX-License-Identifier: MIT
  3. #include <Jolt.h>
  4. #include <Physics/Collision/Shape/CompoundShape.h>
  5. #include <Physics/Collision/CollisionDispatch.h>
  6. #include <Physics/Collision/ShapeCast.h>
  7. #include <Physics/Collision/CastResult.h>
  8. #include <Physics/Collision/TransformedShape.h>
  9. #include <Core/Profiler.h>
  10. #include <Core/StreamIn.h>
  11. #include <Core/StreamOut.h>
  12. #include <ObjectStream/TypeDeclarations.h>
  13. #ifdef JPH_DEBUG_RENDERER
  14. #include <Renderer/DebugRenderer.h>
  15. #endif // JPH_DEBUG_RENDERER
  16. namespace JPH {
  17. JPH_IMPLEMENT_SERIALIZABLE_ABSTRACT(CompoundShapeSettings)
  18. {
  19. JPH_ADD_BASE_CLASS(CompoundShapeSettings, ShapeSettings)
  20. JPH_ADD_ATTRIBUTE(CompoundShapeSettings, mSubShapes)
  21. }
  22. JPH_IMPLEMENT_SERIALIZABLE_NON_VIRTUAL(CompoundShapeSettings::SubShapeSettings)
  23. {
  24. JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mShape)
  25. JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mPosition)
  26. JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mRotation)
  27. JPH_ADD_ATTRIBUTE(CompoundShapeSettings::SubShapeSettings, mUserData)
  28. }
  29. void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const ShapeSettings *inShape, uint32 inUserData)
  30. {
  31. // Add shape
  32. SubShapeSettings shape;
  33. shape.mPosition = inPosition;
  34. shape.mRotation = inRotation;
  35. shape.mShape = inShape;
  36. shape.mUserData = inUserData;
  37. mSubShapes.push_back(shape);
  38. }
  39. void CompoundShapeSettings::AddShape(Vec3Arg inPosition, QuatArg inRotation, const Shape *inShape, uint32 inUserData)
  40. {
  41. // Add shape
  42. SubShapeSettings shape;
  43. shape.mPosition = inPosition;
  44. shape.mRotation = inRotation;
  45. shape.mShapePtr = inShape;
  46. shape.mUserData = inUserData;
  47. mSubShapes.push_back(shape);
  48. }
  49. bool CompoundShape::MustBeStatic() const
  50. {
  51. for (const SubShape &shape : mSubShapes)
  52. if (shape.mShape->MustBeStatic())
  53. return true;
  54. return false;
  55. }
  56. MassProperties CompoundShape::GetMassProperties() const
  57. {
  58. MassProperties p;
  59. // Calculate mass and inertia
  60. p.mMass = 0.0f;
  61. p.mInertia = Mat44::sZero();
  62. for (const SubShape &shape : mSubShapes)
  63. {
  64. // Rotate and translate inertia of child into place
  65. MassProperties child = shape.mShape->GetMassProperties();
  66. child.Rotate(Mat44::sRotation(shape.GetRotation()));
  67. child.Translate(shape.GetPositionCOM());
  68. // Accumulate mass and inertia
  69. p.mMass += child.mMass;
  70. p.mInertia += child.mInertia;
  71. }
  72. // Ensure that inertia is a 3x3 matrix, adding inertias causes the bottom right element to change
  73. p.mInertia.SetColumn4(3, Vec4(0, 0, 0, 1));
  74. return p;
  75. }
  76. AABox CompoundShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
  77. {
  78. if (mSubShapes.size() <= 10)
  79. {
  80. AABox bounds;
  81. for (const SubShape &shape : mSubShapes)
  82. {
  83. Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale);
  84. bounds.Encapsulate(shape.mShape->GetWorldSpaceBounds(transform, shape.TransformScale(inScale)));
  85. }
  86. return bounds;
  87. }
  88. else
  89. {
  90. // If there are too many shapes, use the base class function (this will result in a slightly wider bounding box)
  91. return Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale);
  92. }
  93. }
  94. uint CompoundShape::GetSubShapeIDBitsRecursive() const
  95. {
  96. // Add max of child bits to our bits
  97. uint child_bits = 0;
  98. for (const SubShape &shape : mSubShapes)
  99. child_bits = max(child_bits, shape.mShape->GetSubShapeIDBitsRecursive());
  100. return child_bits + GetSubShapeIDBits();
  101. }
  102. const PhysicsMaterial *CompoundShape::GetMaterial(const SubShapeID &inSubShapeID) const
  103. {
  104. // Decode sub shape index
  105. SubShapeID remainder;
  106. uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
  107. // Pass call on
  108. return mSubShapes[index].mShape->GetMaterial(remainder);
  109. }
  110. uint32 CompoundShape::GetSubShapeUserData(const SubShapeID &inSubShapeID) const
  111. {
  112. // Decode sub shape index
  113. SubShapeID remainder;
  114. uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
  115. // Pass call on
  116. return mSubShapes[index].mShape->GetSubShapeUserData(remainder);
  117. }
  118. TransformedShape CompoundShape::GetSubShapeTransformedShape(const SubShapeID &inSubShapeID, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale, SubShapeID &outRemainder) const
  119. {
  120. // Get the sub shape
  121. const SubShape &sub_shape = mSubShapes[GetSubShapeIndexFromID(inSubShapeID, outRemainder)];
  122. // Calculate transform for sub shape
  123. Vec3 position = inPositionCOM + inRotation * (inScale * sub_shape.GetPositionCOM());
  124. Quat rotation = inRotation * sub_shape.GetRotation();
  125. Vec3 scale = sub_shape.TransformScale(inScale);
  126. // Return transformed shape
  127. TransformedShape ts(position, rotation, sub_shape.mShape, BodyID());
  128. ts.SetShapeScale(scale);
  129. return ts;
  130. }
  131. Vec3 CompoundShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
  132. {
  133. // Decode sub shape index
  134. SubShapeID remainder;
  135. uint32 index = GetSubShapeIndexFromID(inSubShapeID, remainder);
  136. // Transform surface position to local space and pass call on
  137. const SubShape &shape = mSubShapes[index];
  138. Mat44 transform = Mat44::sInverseRotationTranslation(shape.GetRotation(), shape.GetPositionCOM());
  139. Vec3 normal = shape.mShape->GetSurfaceNormal(remainder, transform * inLocalSurfacePosition);
  140. // Transform normal to this shape's space
  141. return transform.Multiply3x3Transposed(normal);
  142. }
  143. void CompoundShape::GetSubmergedVolume(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const Plane &inSurface, float &outTotalVolume, float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
  144. {
  145. outTotalVolume = 0.0f;
  146. outSubmergedVolume = 0.0f;
  147. outCenterOfBuoyancy = Vec3::sZero();
  148. for (const SubShape &shape : mSubShapes)
  149. {
  150. // Get center of mass transform of child
  151. Mat44 transform = inCenterOfMassTransform * shape.GetLocalTransformNoScale(inScale);
  152. // Recurse to child
  153. float total_volume, submerged_volume;
  154. Vec3 center_of_buoyancy;
  155. shape.mShape->GetSubmergedVolume(transform, shape.TransformScale(inScale), inSurface, total_volume, submerged_volume, center_of_buoyancy);
  156. // Accumulate volumes
  157. outTotalVolume += total_volume;
  158. outSubmergedVolume += submerged_volume;
  159. // The center of buoyancy is the weighted average of the center of buoyancy of our child shapes
  160. outCenterOfBuoyancy += submerged_volume * center_of_buoyancy;
  161. }
  162. if (outSubmergedVolume > 0.0f)
  163. outCenterOfBuoyancy /= outSubmergedVolume;
  164. #ifdef JPH_DEBUG_RENDERER
  165. // Draw senter of buoyancy
  166. if (sDrawSubmergedVolumes)
  167. DebugRenderer::sInstance->DrawWireSphere(outCenterOfBuoyancy, 0.05f, Color::sRed, 1);
  168. #endif // JPH_DEBUG_RENDERER
  169. }
  170. #ifdef JPH_DEBUG_RENDERER
  171. void CompoundShape::Draw(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
  172. {
  173. for (const SubShape &shape : mSubShapes)
  174. {
  175. Mat44 transform = shape.GetLocalTransformNoScale(inScale);
  176. shape.mShape->Draw(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inUseMaterialColors, inDrawWireframe);
  177. }
  178. }
  179. void CompoundShape::DrawGetSupportFunction(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inDrawSupportDirection) const
  180. {
  181. for (const SubShape &shape : mSubShapes)
  182. {
  183. Mat44 transform = shape.GetLocalTransformNoScale(inScale);
  184. shape.mShape->DrawGetSupportFunction(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale), inColor, inDrawSupportDirection);
  185. }
  186. }
  187. void CompoundShape::DrawGetSupportingFace(DebugRenderer *inRenderer, Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
  188. {
  189. for (const SubShape &shape : mSubShapes)
  190. {
  191. Mat44 transform = shape.GetLocalTransformNoScale(inScale);
  192. shape.mShape->DrawGetSupportingFace(inRenderer, inCenterOfMassTransform * transform, shape.TransformScale(inScale));
  193. }
  194. }
  195. #endif // JPH_DEBUG_RENDERER
  196. void CompoundShape::TransformShape(Mat44Arg inCenterOfMassTransform, TransformedShapeCollector &ioCollector) const
  197. {
  198. for (const SubShape &shape : mSubShapes)
  199. shape.mShape->TransformShape(inCenterOfMassTransform * Mat44::sRotationTranslation(shape.GetRotation(), shape.GetPositionCOM()), ioCollector);
  200. }
  201. void CompoundShape::sCastCompoundVsShape(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, const Shape *inShape, Vec3Arg inScale, const ShapeFilter &inShapeFilter, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, const SubShapeIDCreator &inSubShapeIDCreator2, CastShapeCollector &ioCollector)
  202. {
  203. JPH_PROFILE_FUNCTION();
  204. // Fetch compound shape from cast shape
  205. JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Compound);
  206. const CompoundShape *compound = static_cast<const CompoundShape *>(inShapeCast.mShape);
  207. // Number of sub shapes
  208. int n = (int)compound->mSubShapes.size();
  209. // Determine amount of bits for sub shape
  210. uint sub_shape_bits = compound->GetSubShapeIDBits();
  211. // Recurse to sub shapes
  212. for (int i = 0; i < n; ++i)
  213. {
  214. const SubShape &shape = compound->mSubShapes[i];
  215. // Create ID for sub shape
  216. SubShapeIDCreator shape1_sub_shape_id = inSubShapeIDCreator1.PushID(i, sub_shape_bits);
  217. // Transform the shape cast and update the shape
  218. Mat44 transform = inShapeCast.mCenterOfMassStart * shape.GetLocalTransformNoScale(inShapeCast.mScale);
  219. Vec3 scale = shape.TransformScale(inShapeCast.mScale);
  220. ShapeCast shape_cast(shape.mShape, scale, transform, inShapeCast.mDirection);
  221. CollisionDispatch::sCastShapeVsShape(shape_cast, inShapeCastSettings, inShape, inScale, inShapeFilter, inCenterOfMassTransform2, shape1_sub_shape_id, inSubShapeIDCreator2, ioCollector);
  222. if (ioCollector.ShouldEarlyOut())
  223. break;
  224. }
  225. }
  226. void CompoundShape::SaveBinaryState(StreamOut &inStream) const
  227. {
  228. Shape::SaveBinaryState(inStream);
  229. inStream.Write(mCenterOfMass);
  230. inStream.Write(mLocalBounds);
  231. inStream.Write(mInnerRadius);
  232. // Write sub shapes
  233. size_t len = mSubShapes.size();
  234. inStream.Write(len);
  235. if (!inStream.IsFailed())
  236. for (size_t i = 0; i < len; ++i)
  237. {
  238. const SubShape &s = mSubShapes[i];
  239. inStream.Write(s.mUserData);
  240. inStream.Write(s.mPositionCOM);
  241. inStream.Write(s.mRotation);
  242. }
  243. }
  244. void CompoundShape::RestoreBinaryState(StreamIn &inStream)
  245. {
  246. Shape::RestoreBinaryState(inStream);
  247. inStream.Read(mCenterOfMass);
  248. inStream.Read(mLocalBounds);
  249. inStream.Read(mInnerRadius);
  250. // Read sub shapes
  251. size_t len = 0;
  252. inStream.Read(len);
  253. if (!inStream.IsEOF() && !inStream.IsFailed())
  254. {
  255. mSubShapes.resize(len);
  256. for (size_t i = 0; i < len; ++i)
  257. {
  258. SubShape &s = mSubShapes[i];
  259. inStream.Read(s.mUserData);
  260. inStream.Read(s.mPositionCOM);
  261. inStream.Read(s.mRotation);
  262. s.mIsRotationIdentity = s.mRotation == Float3(0, 0, 0);
  263. }
  264. }
  265. }
  266. void CompoundShape::SaveSubShapeState(ShapeList &outSubShapes) const
  267. {
  268. outSubShapes.clear();
  269. outSubShapes.reserve(mSubShapes.size());
  270. for (const SubShape &shape : mSubShapes)
  271. outSubShapes.push_back(shape.mShape);
  272. }
  273. void CompoundShape::RestoreSubShapeState(const ShapeRefC *inSubShapes, uint inNumShapes)
  274. {
  275. JPH_ASSERT(mSubShapes.size() == inNumShapes);
  276. for (uint i = 0; i < inNumShapes; ++i)
  277. mSubShapes[i].mShape = inSubShapes[i];
  278. }
  279. Shape::Stats CompoundShape::GetStatsRecursive(VisitedShapes &ioVisitedShapes) const
  280. {
  281. // Get own stats
  282. Stats stats = Shape::GetStatsRecursive(ioVisitedShapes);
  283. // Add child stats
  284. for (const SubShape &shape : mSubShapes)
  285. {
  286. Stats child_stats = shape.mShape->GetStatsRecursive(ioVisitedShapes);
  287. stats.mSizeBytes += child_stats.mSizeBytes;
  288. stats.mNumTriangles += child_stats.mNumTriangles;
  289. }
  290. return stats;
  291. }
  292. float CompoundShape::GetVolume() const
  293. {
  294. float volume = 0.0f;
  295. for (const SubShape &shape : mSubShapes)
  296. volume += shape.mShape->GetVolume();
  297. return volume;
  298. }
  299. bool CompoundShape::IsValidScale(Vec3Arg inScale) const
  300. {
  301. if (!Shape::IsValidScale(inScale))
  302. return false;
  303. for (const SubShape &shape : mSubShapes)
  304. {
  305. // Test if the scale is non-uniform and the shape is rotated
  306. if (!shape.IsValidScale(inScale))
  307. return false;
  308. // Test the child shape
  309. if (!shape.mShape->IsValidScale(shape.TransformScale(inScale)))
  310. return false;
  311. }
  312. return true;
  313. }
  314. void CompoundShape::sRegister()
  315. {
  316. for (EShapeSubType s : sCompoundSubShapeTypes)
  317. CollisionDispatch::sRegisterCastShape(s, sCastCompoundVsShape);
  318. }
  319. } // JPH