Jolt Physics
A multi core friendly Game Physics Engine
Loading...
Searching...
No Matches
PolyhedronSubmergedVolumeCalculator.h
Go to the documentation of this file.
1// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
2// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
3// SPDX-License-Identifier: MIT
4
5#pragma once
6
8#ifdef JPH_DEBUG_RENDERER
10#endif // JPH_DEBUG_RENDERER
11
13
18{
19private:
20 // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 4 vertices submerged
21 // inV1 .. inV4 are submerged
22 inline static void sTetrahedronVolume4(Vec3Arg inV1, Vec3Arg inV2, Vec3Arg inV3, Vec3Arg inV4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
23 {
24 // Calculate center of mass and mass of this tetrahedron,
25 // see: https://en.wikipedia.org/wiki/Tetrahedron#Volume
26 outVolumeTimes6 = max((inV1 - inV4).Dot((inV2 - inV4).Cross(inV3 - inV4)), 0.0f); // All contributions should be positive because we use a reference point that is on the surface of the hull
27 outCenterTimes4 = inV1 + inV2 + inV3 + inV4;
28 }
29
30 // Get the intersection point with a plane.
31 // inV1 is inD1 distance away from the plane, inV2 is inD2 distance away from the plane
32 inline static Vec3 sGetPlaneIntersection(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2)
33 {
34 JPH_ASSERT(Sign(inD1) != Sign(inD2), "Assuming both points are on opposite ends of the plane");
35 float delta = inD1 - inD2;
36 if (abs(delta) < 1.0e-6f)
37 return inV1; // Parallel to plane, just pick a point
38 else
39 return inV1 + inD1 * (inV2 - inV1) / delta;
40 }
41
42 // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 1 vertex submerged
43 // inV1 is submerged, inV2 .. inV4 are not
44 // inD1 .. inD4 are the distances from the points to the plane
45 inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume1(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
46 {
47 // A tetrahedron with 1 point submerged is cut along 3 edges forming a new tetrahedron
48 Vec3 v2 = sGetPlaneIntersection(inV1, inD1, inV2, inD2);
49 Vec3 v3 = sGetPlaneIntersection(inV1, inD1, inV3, inD3);
50 Vec3 v4 = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
51
52 #ifdef JPH_DEBUG_RENDERER
53 // Draw intersection between tetrahedron and surface
55 {
56 RVec3 v2w = mBaseOffset + v2;
57 RVec3 v3w = mBaseOffset + v3;
58 RVec3 v4w = mBaseOffset + v4;
59
62 }
63 #endif // JPH_DEBUG_RENDERER
64
65 sTetrahedronVolume4(inV1, v2, v3, v4, outVolumeTimes6, outCenterTimes4);
66 }
67
68 // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 2 vertices submerged
69 // inV1, inV2 are submerged, inV3, inV4 are not
70 // inD1 .. inD4 are the distances from the points to the plane
71 inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume2(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
72 {
73 // A tetrahedron with 2 points submerged is cut along 4 edges forming a quad
74 Vec3 c = sGetPlaneIntersection(inV1, inD1, inV3, inD3);
75 Vec3 d = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
76 Vec3 e = sGetPlaneIntersection(inV2, inD2, inV4, inD4);
77 Vec3 f = sGetPlaneIntersection(inV2, inD2, inV3, inD3);
78
79 #ifdef JPH_DEBUG_RENDERER
80 // Draw intersection between tetrahedron and surface
82 {
83 RVec3 cw = mBaseOffset + c;
84 RVec3 dw = mBaseOffset + d;
85 RVec3 ew = mBaseOffset + e;
86 RVec3 fw = mBaseOffset + f;
87
92 }
93 #endif // JPH_DEBUG_RENDERER
94
95 // We pick point c as reference (which is on the cut off surface)
96 // This leaves us with three tetrahedrons to sum up (any faces that are in the same plane as c will have zero volume)
97 Vec3 center1, center2, center3;
98 float volume1, volume2, volume3;
99 sTetrahedronVolume4(e, f, inV2, c, volume1, center1);
100 sTetrahedronVolume4(e, inV1, d, c, volume2, center2);
101 sTetrahedronVolume4(e, inV2, inV1, c, volume3, center3);
102
103 // Tally up the totals
104 outVolumeTimes6 = volume1 + volume2 + volume3;
105 outCenterTimes4 = outVolumeTimes6 > 0.0f? (volume1 * center1 + volume2 * center2 + volume3 * center3) / outVolumeTimes6 : Vec3::sZero();
106 }
107
108 // Calculate submerged volume * 6 and center of mass * 4 for a tetrahedron with 3 vertices submerged
109 // inV1, inV2, inV3 are submerged, inV4 is not
110 // inD1 .. inD4 are the distances from the points to the plane
111 inline JPH_IF_NOT_DEBUG_RENDERER(static) void sTetrahedronVolume3(Vec3Arg inV1, float inD1, Vec3Arg inV2, float inD2, Vec3Arg inV3, float inD3, Vec3Arg inV4, float inD4, float &outVolumeTimes6, Vec3 &outCenterTimes4)
112 {
113 // A tetrahedron with 1 point above the surface is cut along 3 edges forming a new tetrahedron
114 Vec3 v1 = sGetPlaneIntersection(inV1, inD1, inV4, inD4);
115 Vec3 v2 = sGetPlaneIntersection(inV2, inD2, inV4, inD4);
116 Vec3 v3 = sGetPlaneIntersection(inV3, inD3, inV4, inD4);
117
118 #ifdef JPH_DEBUG_RENDERER
119 // Draw intersection between tetrahedron and surface
121 {
122 RVec3 v1w = mBaseOffset + v1;
123 RVec3 v2w = mBaseOffset + v2;
124 RVec3 v3w = mBaseOffset + v3;
125
128 }
129 #endif // JPH_DEBUG_RENDERER
130
131 Vec3 dry_center, total_center;
132 float dry_volume, total_volume;
133
134 // We first calculate the part that is above the surface
135 sTetrahedronVolume4(v1, v2, v3, inV4, dry_volume, dry_center);
136
137 // Calculate the total volume
138 sTetrahedronVolume4(inV1, inV2, inV3, inV4, total_volume, total_center);
139
140 // From this we can calculate the center and volume of the submerged part
141 outVolumeTimes6 = max(total_volume - dry_volume, 0.0f);
142 outCenterTimes4 = outVolumeTimes6 > 0.0f? (total_center * total_volume - dry_center * dry_volume) / outVolumeTimes6 : Vec3::sZero();
143 }
144
145public:
147 class Point
148 {
149 public:
153 };
154
162#ifdef JPH_DEBUG_RENDERER
164#endif // JPH_DEBUG_RENDERER
165 PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer
166#ifdef JPH_DEBUG_RENDERER // Not using JPH_IF_DEBUG_RENDERER for Doxygen
167 , RVec3 inBaseOffset
168#endif // JPH_DEBUG_RENDERER
169 ) :
170 mPoints(ioBuffer)
171#ifdef JPH_DEBUG_RENDERER
172 , mBaseOffset(inBaseOffset)
173#endif // JPH_DEBUG_RENDERER
174 {
175 // Convert the points to world space and determine the distance to the surface
176 float reference_dist = FLT_MAX;
177 for (int p = 0; p < inNumPoints; ++p)
178 {
179 // Calculate values
180 Vec3 transformed_point = inTransform * *reinterpret_cast<const Vec3 *>(reinterpret_cast<const uint8 *>(inPoints) + p * inPointStride);
181 float dist = inSurface.SignedDistance(transformed_point);
182 bool above = dist >= 0.0f;
183
184 // Keep track if all are above or below
185 mAllAbove &= above;
186 mAllBelow &= !above;
187
188 // Calculate lowest point, we use this to create tetrahedrons out of all faces
189 if (reference_dist > dist)
190 {
191 mReferencePointIdx = p;
192 reference_dist = dist;
193 }
194
195 // Store values
196 ioBuffer->mPosition = transformed_point;
197 ioBuffer->mDistanceToSurface = dist;
198 ioBuffer->mAboveSurface = above;
199 ++ioBuffer;
200 }
201 }
202
204 inline bool AreAllAbove() const
205 {
206 return mAllAbove;
207 }
208
210 inline bool AreAllBelow() const
211 {
212 return mAllBelow;
213 }
214
216 inline int GetReferencePointIdx() const
217 {
218 return mReferencePointIdx;
219 }
220
222 void AddFace(int inIdx1, int inIdx2, int inIdx3)
223 {
224 JPH_ASSERT(inIdx1 != mReferencePointIdx && inIdx2 != mReferencePointIdx && inIdx3 != mReferencePointIdx, "A face using the reference point will not contribute to the volume");
225
226 // Find the points
227 const Point &ref = mPoints[mReferencePointIdx];
228 const Point &p1 = mPoints[inIdx1];
229 const Point &p2 = mPoints[inIdx2];
230 const Point &p3 = mPoints[inIdx3];
231
232 // Determine which vertices are submerged
233 uint code = (p1.mAboveSurface? 0 : 0b001) | (p2.mAboveSurface? 0 : 0b010) | (p3.mAboveSurface? 0 : 0b100);
234
235 float volume;
236 Vec3 center;
237 switch (code)
238 {
239 case 0b000:
240 // One point submerged
241 sTetrahedronVolume1(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
242 break;
243
244 case 0b001:
245 // Two points submerged
246 sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center);
247 break;
248
249 case 0b010:
250 // Two points submerged
251 sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center);
252 break;
253
254 case 0b100:
255 // Two points submerged
256 sTetrahedronVolume2(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
257 break;
258
259 case 0b011:
260 // Three points submerged
261 sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, volume, center);
262 break;
263
264 case 0b101:
265 // Three points submerged
266 sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, volume, center);
267 break;
268
269 case 0b110:
270 // Three points submerged
271 sTetrahedronVolume3(ref.mPosition, ref.mDistanceToSurface, p3.mPosition, p3.mDistanceToSurface, p2.mPosition, p2.mDistanceToSurface, p1.mPosition, p1.mDistanceToSurface, volume, center);
272 break;
273
274 case 0b111:
275 // Four points submerged
276 sTetrahedronVolume4(ref.mPosition, p3.mPosition, p2.mPosition, p1.mPosition, volume, center);
277 break;
278
279 default:
280 // Should not be possible
281 JPH_ASSERT(false);
282 volume = 0.0f;
283 center = Vec3::sZero();
284 break;
285 }
286
287 mSubmergedVolume += volume;
288 mCenterOfBuoyancy += volume * center;
289 }
290
292 void GetResult(float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
293 {
294 outCenterOfBuoyancy = mSubmergedVolume > 0.0f? mCenterOfBuoyancy / (4.0f * mSubmergedVolume) : Vec3::sZero(); // Do this before dividing submerged volume by 6 to get correct weight factor
295 outSubmergedVolume = mSubmergedVolume / 6.0f;
296 }
297
298private:
299 // The precalculated points for this polyhedron
300 const Point * mPoints;
301
302 // If all points are above/below the surface
303 bool mAllBelow = true;
304 bool mAllAbove = true;
305
306 // The lowest point
307 int mReferencePointIdx = 0;
308
309 // Aggregator for submerged volume and center of buoyancy
310 float mSubmergedVolume = 0.0f;
311 Vec3 mCenterOfBuoyancy = Vec3::sZero();
312
313#ifdef JPH_DEBUG_RENDERER
314 // Base offset used for drawing
315 RVec3 mBaseOffset;
316#endif
317};
318
std::uint8_t uint8
Definition: Core.h:440
#define JPH_IF_NOT_DEBUG_RENDERER(...)
Definition: Core.h:511
unsigned int uint
Definition: Core.h:439
#define JPH_NAMESPACE_END
Definition: Core.h:367
#define JPH_NAMESPACE_BEGIN
Definition: Core.h:361
#define JPH_ASSERT(...)
Definition: IssueReporting.h:33
constexpr T Sign(T inV)
Get the sign of a value.
Definition: Math.h:66
static const Color sGreen
Definition: Color.h:54
static const Color sWhite
Definition: Color.h:64
void DrawWireTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor)
Draw wireframe triangle.
Definition: DebugRenderer.cpp:242
static DebugRenderer * sInstance
Singleton instance.
Definition: DebugRenderer.h:168
virtual void DrawTriangle(RVec3Arg inV1, RVec3Arg inV2, RVec3Arg inV3, ColorArg inColor, ECastShadow inCastShadow=ECastShadow::Off)=0
Draw a single back face culled triangle.
Holds a 4x4 matrix of floats, but supports also operations on the 3x3 upper left part of the matrix.
Definition: Mat44.h:13
An infinite plane described by the formula X . Normal + Constant = 0.
Definition: Plane.h:11
float SignedDistance(Vec3Arg inPoint) const
Distance point to plane.
Definition: Plane.h:46
A helper class that contains cached information about a polyhedron vertex.
Definition: PolyhedronSubmergedVolumeCalculator.h:148
Vec3 mPosition
World space position of vertex.
Definition: PolyhedronSubmergedVolumeCalculator.h:150
float mDistanceToSurface
Signed distance to the surface (> 0 is above, < 0 is below)
Definition: PolyhedronSubmergedVolumeCalculator.h:151
bool mAboveSurface
If the point is above the surface (mDistanceToSurface > 0)
Definition: PolyhedronSubmergedVolumeCalculator.h:152
Definition: PolyhedronSubmergedVolumeCalculator.h:18
void AddFace(int inIdx1, int inIdx2, int inIdx3)
Add a polyhedron face. Supply the indices of the points that form the face (in counter clockwise orde...
Definition: PolyhedronSubmergedVolumeCalculator.h:222
PolyhedronSubmergedVolumeCalculator(const Mat44 &inTransform, const Vec3 *inPoints, int inPointStride, int inNumPoints, const Plane &inSurface, Point *ioBuffer, RVec3 inBaseOffset)
Definition: PolyhedronSubmergedVolumeCalculator.h:165
void GetResult(float &outSubmergedVolume, Vec3 &outCenterOfBuoyancy) const
Call after all faces have been added. Returns the submerged volume and the center of buoyancy for the...
Definition: PolyhedronSubmergedVolumeCalculator.h:292
bool AreAllBelow() const
Check if all points are below the surface. Should be used as early out.
Definition: PolyhedronSubmergedVolumeCalculator.h:210
int GetReferencePointIdx() const
Get the lowest point of the polyhedron. Used to form the 4th vertex to make a tetrahedron out of a po...
Definition: PolyhedronSubmergedVolumeCalculator.h:216
bool AreAllAbove() const
Check if all points are above the surface. Should be used as early out.
Definition: PolyhedronSubmergedVolumeCalculator.h:204
static bool sDrawSubmergedVolumes
Debug helper which draws the intersection between water and the shapes, the center of buoyancy and th...
Definition: Shape.h:431
Definition: Vec3.h:16
static JPH_INLINE Vec3 sZero()
Vector with all zeros.
Definition: Vec3.inl:107