2
0

TaperedCapsuleShape.cpp 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453
  1. // Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
  2. // SPDX-FileCopyrightText: 2021 Jorrit Rouwe
  3. // SPDX-License-Identifier: MIT
  4. #include <Jolt/Jolt.h>
  5. #include <Jolt/Physics/Collision/Shape/TaperedCapsuleShape.h>
  6. #include <Jolt/Physics/Collision/Shape/SphereShape.h>
  7. #include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
  8. #include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
  9. #include <Jolt/Physics/Collision/TransformedShape.h>
  10. #include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
  11. #include <Jolt/Geometry/RayCapsule.h>
  12. #include <Jolt/ObjectStream/TypeDeclarations.h>
  13. #include <Jolt/Core/StreamIn.h>
  14. #include <Jolt/Core/StreamOut.h>
  15. #ifdef JPH_DEBUG_RENDERER
  16. #include <Jolt/Renderer/DebugRenderer.h>
  17. #endif // JPH_DEBUG_RENDERER
  18. JPH_NAMESPACE_BEGIN
  19. JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCapsuleShapeSettings)
  20. {
  21. JPH_ADD_BASE_CLASS(TaperedCapsuleShapeSettings, ConvexShapeSettings)
  22. JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mHalfHeightOfTaperedCylinder)
  23. JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mTopRadius)
  24. JPH_ADD_ATTRIBUTE(TaperedCapsuleShapeSettings, mBottomRadius)
  25. }
  26. bool TaperedCapsuleShapeSettings::IsSphere() const
  27. {
  28. return max(mTopRadius, mBottomRadius) >= 2.0f * mHalfHeightOfTaperedCylinder + min(mTopRadius, mBottomRadius);
  29. }
  30. ShapeSettings::ShapeResult TaperedCapsuleShapeSettings::Create() const
  31. {
  32. if (mCachedResult.IsEmpty())
  33. {
  34. Ref<Shape> shape;
  35. if (IsValid() && IsSphere())
  36. {
  37. // Determine sphere center and radius
  38. float radius, center;
  39. if (mTopRadius > mBottomRadius)
  40. {
  41. radius = mTopRadius;
  42. center = mHalfHeightOfTaperedCylinder;
  43. }
  44. else
  45. {
  46. radius = mBottomRadius;
  47. center = -mHalfHeightOfTaperedCylinder;
  48. }
  49. // Create sphere
  50. shape = new SphereShape(radius, mMaterial);
  51. // Offset sphere if needed
  52. if (abs(center) > 1.0e-6f)
  53. {
  54. RotatedTranslatedShapeSettings rot_trans(Vec3(0, center, 0), Quat::sIdentity(), shape);
  55. mCachedResult = rot_trans.Create();
  56. }
  57. else
  58. mCachedResult.Set(shape);
  59. }
  60. else
  61. {
  62. // Normal tapered capsule shape
  63. shape = new TaperedCapsuleShape(*this, mCachedResult);
  64. }
  65. }
  66. return mCachedResult;
  67. }
  68. TaperedCapsuleShapeSettings::TaperedCapsuleShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, const PhysicsMaterial *inMaterial) :
  69. ConvexShapeSettings(inMaterial),
  70. mHalfHeightOfTaperedCylinder(inHalfHeightOfTaperedCylinder),
  71. mTopRadius(inTopRadius),
  72. mBottomRadius(inBottomRadius)
  73. {
  74. }
  75. TaperedCapsuleShape::TaperedCapsuleShape(const TaperedCapsuleShapeSettings &inSettings, ShapeResult &outResult) :
  76. ConvexShape(EShapeSubType::TaperedCapsule, inSettings, outResult),
  77. mTopRadius(inSettings.mTopRadius),
  78. mBottomRadius(inSettings.mBottomRadius)
  79. {
  80. if (mTopRadius <= 0.0f)
  81. {
  82. outResult.SetError("Invalid top radius");
  83. return;
  84. }
  85. if (mBottomRadius <= 0.0f)
  86. {
  87. outResult.SetError("Invalid bottom radius");
  88. return;
  89. }
  90. if (inSettings.mHalfHeightOfTaperedCylinder <= 0.0f)
  91. {
  92. outResult.SetError("Invalid height");
  93. return;
  94. }
  95. // If this goes off one of the sphere ends falls totally inside the other and you should use a sphere instead
  96. if (inSettings.IsSphere())
  97. {
  98. outResult.SetError("One sphere embedded in other sphere, please use sphere shape instead");
  99. return;
  100. }
  101. // Approximation: The center of mass is exactly half way between the top and bottom cap of the tapered capsule
  102. mTopCenter = inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius);
  103. mBottomCenter = -inSettings.mHalfHeightOfTaperedCylinder + 0.5f * (mBottomRadius - mTopRadius);
  104. // Calculate center of mass
  105. mCenterOfMass = Vec3(0, inSettings.mHalfHeightOfTaperedCylinder - mTopCenter, 0);
  106. // Calculate convex radius
  107. mConvexRadius = min(mTopRadius, mBottomRadius);
  108. JPH_ASSERT(mConvexRadius > 0.0f);
  109. // Calculate the sin and tan of the angle that the cone surface makes with the Y axis
  110. // See: TaperedCapsuleShape.gliffy
  111. mSinAlpha = (mBottomRadius - mTopRadius) / (mTopCenter - mBottomCenter);
  112. JPH_ASSERT(mSinAlpha >= -1.0f && mSinAlpha <= 1.0f);
  113. mTanAlpha = Tan(ASin(mSinAlpha));
  114. outResult.Set(this);
  115. }
  116. class TaperedCapsuleShape::TaperedCapsule final : public Support
  117. {
  118. public:
  119. TaperedCapsule(Vec3Arg inTopCenter, Vec3Arg inBottomCenter, float inTopRadius, float inBottomRadius, float inConvexRadius) :
  120. mTopCenter(inTopCenter),
  121. mBottomCenter(inBottomCenter),
  122. mTopRadius(inTopRadius),
  123. mBottomRadius(inBottomRadius),
  124. mConvexRadius(inConvexRadius)
  125. {
  126. static_assert(sizeof(TaperedCapsule) <= sizeof(SupportBuffer), "Buffer size too small");
  127. JPH_ASSERT(IsAligned(this, alignof(TaperedCapsule)));
  128. }
  129. virtual Vec3 GetSupport(Vec3Arg inDirection) const override
  130. {
  131. // Check zero vector
  132. float len = inDirection.Length();
  133. if (len == 0.0f)
  134. return mTopCenter + Vec3(0, mTopRadius, 0); // Return top
  135. // Check if the support of the top sphere or bottom sphere is bigger
  136. Vec3 support_top = mTopCenter + (mTopRadius / len) * inDirection;
  137. Vec3 support_bottom = mBottomCenter + (mBottomRadius / len) * inDirection;
  138. if (support_top.Dot(inDirection) > support_bottom.Dot(inDirection))
  139. return support_top;
  140. else
  141. return support_bottom;
  142. }
  143. virtual float GetConvexRadius() const override
  144. {
  145. return mConvexRadius;
  146. }
  147. private:
  148. Vec3 mTopCenter;
  149. Vec3 mBottomCenter;
  150. float mTopRadius;
  151. float mBottomRadius;
  152. float mConvexRadius;
  153. };
  154. const ConvexShape::Support *TaperedCapsuleShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
  155. {
  156. JPH_ASSERT(IsValidScale(inScale));
  157. // Get scaled tapered capsule
  158. Vec3 abs_scale = inScale.Abs();
  159. float scale_xz = abs_scale.GetX();
  160. float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule
  161. Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0);
  162. Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0);
  163. float scaled_top_radius = scale_xz * mTopRadius;
  164. float scaled_bottom_radius = scale_xz * mBottomRadius;
  165. float scaled_convex_radius = scale_xz * mConvexRadius;
  166. switch (inMode)
  167. {
  168. case ESupportMode::IncludeConvexRadius:
  169. return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, scaled_top_radius, scaled_bottom_radius, 0.0f);
  170. case ESupportMode::ExcludeConvexRadius:
  171. case ESupportMode::Default:
  172. {
  173. // Get radii reduced by convex radius
  174. float tr = scaled_top_radius - scaled_convex_radius;
  175. float br = scaled_bottom_radius - scaled_convex_radius;
  176. JPH_ASSERT(tr >= 0.0f && br >= 0.0f);
  177. JPH_ASSERT(tr == 0.0f || br == 0.0f, "Convex radius should be that of the smallest sphere");
  178. return new (&inBuffer) TaperedCapsule(scaled_top_center, scaled_bottom_center, tr, br, scaled_convex_radius);
  179. }
  180. }
  181. JPH_ASSERT(false);
  182. return nullptr;
  183. }
  184. void TaperedCapsuleShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
  185. {
  186. JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
  187. JPH_ASSERT(IsValidScale(inScale));
  188. // Check zero vector
  189. float len = inDirection.Length();
  190. if (len == 0.0f)
  191. return;
  192. // Get scaled tapered capsule
  193. Vec3 abs_scale = inScale.Abs();
  194. float scale_xz = abs_scale.GetX();
  195. float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule
  196. Vec3 scaled_top_center = Vec3(0, scale_y * mTopCenter, 0);
  197. Vec3 scaled_bottom_center = Vec3(0, scale_y * mBottomCenter, 0);
  198. float scaled_top_radius = scale_xz * mTopRadius;
  199. float scaled_bottom_radius = scale_xz * mBottomRadius;
  200. // Get support point for top and bottom sphere in the opposite of inDirection (including convex radius)
  201. Vec3 support_top = scaled_top_center - (scaled_top_radius / len) * inDirection;
  202. Vec3 support_bottom = scaled_bottom_center - (scaled_bottom_radius / len) * inDirection;
  203. // Get projection on inDirection
  204. float proj_top = support_top.Dot(inDirection);
  205. float proj_bottom = support_bottom.Dot(inDirection);
  206. // If projection is roughly equal then return line, otherwise we return nothing as there's only 1 point
  207. if (abs(proj_top - proj_bottom) < cCapsuleProjectionSlop * len)
  208. {
  209. outVertices.push_back(inCenterOfMassTransform * support_top);
  210. outVertices.push_back(inCenterOfMassTransform * support_bottom);
  211. }
  212. }
  213. MassProperties TaperedCapsuleShape::GetMassProperties() const
  214. {
  215. AABox box = GetInertiaApproximation();
  216. MassProperties p;
  217. p.SetMassAndInertiaOfSolidBox(box.GetSize(), GetDensity());
  218. return p;
  219. }
  220. Vec3 TaperedCapsuleShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
  221. {
  222. JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
  223. // See: TaperedCapsuleShape.gliffy
  224. // We need to calculate ty and by in order to see if the position is on the top or bottom sphere
  225. // sin(alpha) = by / br = ty / tr
  226. // => by = sin(alpha) * br, ty = sin(alpha) * tr
  227. if (inLocalSurfacePosition.GetY() > mTopCenter + mSinAlpha * mTopRadius)
  228. return (inLocalSurfacePosition - Vec3(0, mTopCenter, 0)).Normalized();
  229. else if (inLocalSurfacePosition.GetY() < mBottomCenter + mSinAlpha * mBottomRadius)
  230. return (inLocalSurfacePosition - Vec3(0, mBottomCenter, 0)).Normalized();
  231. else
  232. {
  233. // Get perpendicular vector to the surface in the xz plane
  234. Vec3 perpendicular = Vec3(inLocalSurfacePosition.GetX(), 0, inLocalSurfacePosition.GetZ()).NormalizedOr(Vec3::sAxisX());
  235. // We know that the perpendicular has length 1 and that it needs a y component where tan(alpha) = y / 1 in order to align it to the surface
  236. perpendicular.SetY(mTanAlpha);
  237. return perpendicular.Normalized();
  238. }
  239. }
  240. AABox TaperedCapsuleShape::GetLocalBounds() const
  241. {
  242. float max_radius = max(mTopRadius, mBottomRadius);
  243. return AABox(Vec3(-max_radius, mBottomCenter - mBottomRadius, -max_radius), Vec3(max_radius, mTopCenter + mTopRadius, max_radius));
  244. }
  245. AABox TaperedCapsuleShape::GetWorldSpaceBounds(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale) const
  246. {
  247. JPH_ASSERT(IsValidScale(inScale));
  248. Vec3 abs_scale = inScale.Abs();
  249. float scale_xz = abs_scale.GetX();
  250. float scale_y = inScale.GetY(); // The sign of y is important as it flips the tapered capsule
  251. Vec3 bottom_extent = Vec3::sReplicate(scale_xz * mBottomRadius);
  252. Vec3 bottom_center = inCenterOfMassTransform * Vec3(0, scale_y * mBottomCenter, 0);
  253. Vec3 top_extent = Vec3::sReplicate(scale_xz * mTopRadius);
  254. Vec3 top_center = inCenterOfMassTransform * Vec3(0, scale_y * mTopCenter, 0);
  255. Vec3 p1 = Vec3::sMin(top_center - top_extent, bottom_center - bottom_extent);
  256. Vec3 p2 = Vec3::sMax(top_center + top_extent, bottom_center + bottom_extent);
  257. return AABox(p1, p2);
  258. }
  259. void TaperedCapsuleShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
  260. {
  261. JPH_ASSERT(IsValidScale(inScale));
  262. Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
  263. // Get scaled tapered capsule
  264. Vec3 abs_scale = inScale.Abs();
  265. float scale_y = abs_scale.GetY();
  266. float scale_xz = abs_scale.GetX();
  267. Vec3 scale_y_flip(1, Sign(inScale.GetY()), 1);
  268. Vec3 scaled_top_center(0, scale_y * mTopCenter, 0);
  269. Vec3 scaled_bottom_center(0, scale_y * mBottomCenter, 0);
  270. float scaled_top_radius = scale_xz * mTopRadius;
  271. float scaled_bottom_radius = scale_xz * mBottomRadius;
  272. for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
  273. if (v.GetInvMass() > 0.0f)
  274. {
  275. Vec3 local_pos = scale_y_flip * (inverse_transform * v.GetPosition());
  276. Vec3 position, normal;
  277. // If the vertex is inside the cone starting at the top center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the top sphere
  278. // This corresponds to: Dot(y-axis, (local_pos - top_center) / |local_pos - top_center|) >= cos(PI/2 - alpha)
  279. // <=> (local_pos - top_center).y >= sin(alpha) * |local_pos - top_center|
  280. Vec3 top_center_to_local_pos = local_pos - scaled_top_center;
  281. float top_center_to_local_pos_len = top_center_to_local_pos.Length();
  282. if (top_center_to_local_pos.GetY() >= mSinAlpha * top_center_to_local_pos_len)
  283. {
  284. // Top sphere
  285. normal = top_center_to_local_pos_len != 0.0f? top_center_to_local_pos / top_center_to_local_pos_len : Vec3::sAxisY();
  286. position = scaled_top_center + scaled_top_radius * normal;
  287. }
  288. else
  289. {
  290. // If the vertex is outside the cone starting at the bottom center pointing along the y-axis with angle PI/2 - alpha then the closest point is on the bottom sphere
  291. // This corresponds to: Dot(y-axis, (local_pos - bottom_center) / |local_pos - bottom_center|) <= cos(PI/2 - alpha)
  292. // <=> (local_pos - bottom_center).y <= sin(alpha) * |local_pos - bottom_center|
  293. Vec3 bottom_center_to_local_pos = local_pos - scaled_bottom_center;
  294. float bottom_center_to_local_pos_len = bottom_center_to_local_pos.Length();
  295. if (bottom_center_to_local_pos.GetY() <= mSinAlpha * bottom_center_to_local_pos_len)
  296. {
  297. // Bottom sphere
  298. normal = bottom_center_to_local_pos_len != 0.0f? bottom_center_to_local_pos / bottom_center_to_local_pos_len : -Vec3::sAxisY();
  299. }
  300. else
  301. {
  302. // Tapered cylinder
  303. normal = Vec3(local_pos.GetX(), 0, local_pos.GetZ()).NormalizedOr(Vec3::sAxisX());
  304. normal.SetY(mTanAlpha);
  305. normal = normal.NormalizedOr(Vec3::sAxisX());
  306. }
  307. position = scaled_bottom_center + scaled_bottom_radius * normal;
  308. }
  309. Plane plane = Plane::sFromPointAndNormal(position, normal);
  310. float penetration = -plane.SignedDistance(local_pos);
  311. if (v.UpdatePenetration(penetration))
  312. {
  313. // Need to flip the normal's y if capsule is flipped (this corresponds to flipping both the point and the normal around y)
  314. plane.SetNormal(scale_y_flip * plane.GetNormal());
  315. // Store collision
  316. v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
  317. }
  318. }
  319. }
  320. #ifdef JPH_DEBUG_RENDERER
  321. void TaperedCapsuleShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
  322. {
  323. if (mGeometry == nullptr)
  324. {
  325. SupportBuffer buffer;
  326. const Support *support = GetSupportFunction(ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
  327. mGeometry = inRenderer->CreateTriangleGeometryForConvex([support](Vec3Arg inDirection) { return support->GetSupport(inDirection); });
  328. }
  329. // Preserve flip along y axis but make sure we're not inside out
  330. Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? inScale.FlipSign<-1, 1, 1>() : inScale;
  331. RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
  332. AABox bounds = Shape::GetWorldSpaceBounds(inCenterOfMassTransform, inScale);
  333. float lod_scale_sq = Square(max(mTopRadius, mBottomRadius));
  334. Color color = inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor;
  335. DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
  336. inRenderer->DrawGeometry(world_transform, bounds, lod_scale_sq, color, mGeometry, DebugRenderer::ECullMode::CullBackFace, DebugRenderer::ECastShadow::On, draw_mode);
  337. }
  338. #endif // JPH_DEBUG_RENDERER
  339. AABox TaperedCapsuleShape::GetInertiaApproximation() const
  340. {
  341. // TODO: For now the mass and inertia is that of a box
  342. float avg_radius = 0.5f * (mTopRadius + mBottomRadius);
  343. return AABox(Vec3(-avg_radius, mBottomCenter - mBottomRadius, -avg_radius), Vec3(avg_radius, mTopCenter + mTopRadius, avg_radius));
  344. }
  345. void TaperedCapsuleShape::SaveBinaryState(StreamOut &inStream) const
  346. {
  347. ConvexShape::SaveBinaryState(inStream);
  348. inStream.Write(mCenterOfMass);
  349. inStream.Write(mTopRadius);
  350. inStream.Write(mBottomRadius);
  351. inStream.Write(mTopCenter);
  352. inStream.Write(mBottomCenter);
  353. inStream.Write(mConvexRadius);
  354. inStream.Write(mSinAlpha);
  355. inStream.Write(mTanAlpha);
  356. }
  357. void TaperedCapsuleShape::RestoreBinaryState(StreamIn &inStream)
  358. {
  359. ConvexShape::RestoreBinaryState(inStream);
  360. inStream.Read(mCenterOfMass);
  361. inStream.Read(mTopRadius);
  362. inStream.Read(mBottomRadius);
  363. inStream.Read(mTopCenter);
  364. inStream.Read(mBottomCenter);
  365. inStream.Read(mConvexRadius);
  366. inStream.Read(mSinAlpha);
  367. inStream.Read(mTanAlpha);
  368. }
  369. bool TaperedCapsuleShape::IsValidScale(Vec3Arg inScale) const
  370. {
  371. return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScale(inScale.Abs());
  372. }
  373. Vec3 TaperedCapsuleShape::MakeScaleValid(Vec3Arg inScale) const
  374. {
  375. Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
  376. return scale.GetSign() * ScaleHelpers::MakeUniformScale(scale.Abs());
  377. }
  378. void TaperedCapsuleShape::sRegister()
  379. {
  380. ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCapsule);
  381. f.mConstruct = []() -> Shape * { return new TaperedCapsuleShape; };
  382. f.mColor = Color::sGreen;
  383. }
  384. JPH_NAMESPACE_END