// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
// SPDX-License-Identifier: MIT
#include <Jolt/Jolt.h>
#include <Jolt/Physics/Collision/Shape/TaperedCylinderShape.h>
#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
#include <Jolt/Physics/Collision/CollidePointResult.h>
#include <Jolt/Physics/Collision/TransformedShape.h>
#include <Jolt/Physics/Collision/CollideSoftBodyVertexIterator.h>
#include <Jolt/ObjectStream/TypeDeclarations.h>
#include <Jolt/Core/StreamIn.h>
#include <Jolt/Core/StreamOut.h>
#ifdef JPH_DEBUG_RENDERER
#include <Jolt/Renderer/DebugRenderer.h>
#endif // JPH_DEBUG_RENDERER
JPH_NAMESPACE_BEGIN
// Approximation of a face of the tapered cylinder
static const Vec3 cTaperedCylinderFace[] =
{
Vec3(0.0f, 0.0f, 1.0f),
Vec3(0.707106769f, 0.0f, 0.707106769f),
Vec3(1.0f, 0.0f, 0.0f),
Vec3(0.707106769f, 0.0f, -0.707106769f),
Vec3(-0.0f, 0.0f, -1.0f),
Vec3(-0.707106769f, 0.0f, -0.707106769f),
Vec3(-1.0f, 0.0f, 0.0f),
Vec3(-0.707106769f, 0.0f, 0.707106769f)
};
JPH_IMPLEMENT_SERIALIZABLE_VIRTUAL(TaperedCylinderShapeSettings)
{
JPH_ADD_BASE_CLASS(TaperedCylinderShapeSettings, ConvexShapeSettings)
JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mHalfHeight)
JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mTopRadius)
JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mBottomRadius)
JPH_ADD_ATTRIBUTE(TaperedCylinderShapeSettings, mConvexRadius)
}
ShapeSettings::ShapeResult TaperedCylinderShapeSettings::Create() const
{
if (mCachedResult.IsEmpty())
{
Ref<Shape> shape;
if (mTopRadius == mBottomRadius)
{
// Convert to regular cylinder
CylinderShapeSettings settings;
settings.mHalfHeight = mHalfHeight;
settings.mRadius = mTopRadius;
settings.mMaterial = mMaterial;
settings.mConvexRadius = mConvexRadius;
shape = new CylinderShape(settings, mCachedResult);
}
else
{
// Normal tapered cylinder shape
shape = new TaperedCylinderShape(*this, mCachedResult);
}
}
return mCachedResult;
}
TaperedCylinderShapeSettings::TaperedCylinderShapeSettings(float inHalfHeightOfTaperedCylinder, float inTopRadius, float inBottomRadius, float inConvexRadius, const PhysicsMaterial *inMaterial) :
ConvexShapeSettings(inMaterial),
mHalfHeight(inHalfHeightOfTaperedCylinder),
mTopRadius(inTopRadius),
mBottomRadius(inBottomRadius),
mConvexRadius(inConvexRadius)
{
}
TaperedCylinderShape::TaperedCylinderShape(const TaperedCylinderShapeSettings &inSettings, ShapeResult &outResult) :
ConvexShape(EShapeSubType::TaperedCylinder, inSettings, outResult),
mTopRadius(inSettings.mTopRadius),
mBottomRadius(inSettings.mBottomRadius),
mConvexRadius(min(inSettings.mConvexRadius, min(inSettings.mTopRadius, inSettings.mBottomRadius)))
{
if (mTopRadius < 0.0f)
{
outResult.SetError("Invalid top radius");
return;
}
if (mBottomRadius < 0.0f)
{
outResult.SetError("Invalid bottom radius");
return;
}
if (mConvexRadius < 0.0f)
{
outResult.SetError("Invalid convex radius");
return;
}
if (inSettings.mHalfHeight <= 0.0f)
{
outResult.SetError("Invalid height");
return;
}
// Calculate the center of mass (using wxMaxima).
// Radius of cross section for tapered cylinder from 0 to h:
// r(x):=br+x*(tr-br)/h;
// Area:
// area(x):=%pi*r(x)^2;
// Total volume of cylinder:
// volume(h):=integrate(area(x),x,0,h);
// Center of mass:
// com(br,tr,h):=integrate(x*area(x),x,0,h)/volume(h);
// Results:
// ratsimp(com(br,tr,h),br,bt);
// Non-tapered cylinder should have com = 0.5:
// ratsimp(com(r,r,h));
// Cone with tip at origin and height h should have com = 3/4 h
// ratsimp(com(0,r,h));
float h = 2.0f * inSettings.mHalfHeight;
float tr = mTopRadius;
float tr2 = Square(tr);
float br = mBottomRadius;
float br2 = Square(br);
float com = h * (3 * tr2 + 2 * br * tr + br2) / (4.0f * (tr2 + br * tr + br2));
mTop = h - com;
mBottom = -com;
outResult.Set(this);
}
class TaperedCylinderShape::TaperedCylinder final : public Support
{
public:
TaperedCylinder(float inTop, float inBottom, float inTopRadius, float inBottomRadius, float inConvexRadius) :
mTop(inTop),
mBottom(inBottom),
mTopRadius(inTopRadius),
mBottomRadius(inBottomRadius),
mConvexRadius(inConvexRadius)
{
static_assert(sizeof(TaperedCylinder) <= sizeof(SupportBuffer), "Buffer size too small");
JPH_ASSERT(IsAligned(this, alignof(TaperedCylinder)));
}
virtual Vec3 GetSupport(Vec3Arg inDirection) const override
{
float x = inDirection.GetX(), y = inDirection.GetY(), z = inDirection.GetZ();
float o = sqrt(Square(x) + Square(z));
if (o > 0.0f)
{
Vec3 top_support((mTopRadius * x) / o, mTop, (mTopRadius * z) / o);
Vec3 bottom_support((mBottomRadius * x) / o, mBottom, (mBottomRadius * z) / o);
return inDirection.Dot(top_support) > inDirection.Dot(bottom_support)? top_support : bottom_support;
}
else
{
if (y > 0.0f)
return Vec3(0, mTop, 0);
else
return Vec3(0, mBottom, 0);
}
}
virtual float GetConvexRadius() const override
{
return mConvexRadius;
}
private:
float mTop;
float mBottom;
float mTopRadius;
float mBottomRadius;
float mConvexRadius;
};
JPH_INLINE void TaperedCylinderShape::GetScaled(Vec3Arg inScale, float &outTop, float &outBottom, float &outTopRadius, float &outBottomRadius, float &outConvexRadius) const
{
Vec3 abs_scale = inScale.Abs();
float scale_xz = abs_scale.GetX();
float scale_y = inScale.GetY();
outTop = scale_y * mTop;
outBottom = scale_y * mBottom;
outTopRadius = scale_xz * mTopRadius;
outBottomRadius = scale_xz * mBottomRadius;
outConvexRadius = min(abs_scale.GetY(), scale_xz) * mConvexRadius;
// Negative Y-scale flips the top and bottom
if (outBottom > outTop)
{
std::swap(outTop, outBottom);
std::swap(outTopRadius, outBottomRadius);
}
}
const ConvexShape::Support *TaperedCylinderShape::GetSupportFunction(ESupportMode inMode, SupportBuffer &inBuffer, Vec3Arg inScale) const
{
JPH_ASSERT(IsValidScale(inScale));
// Get scaled tapered cylinder
float top, bottom, top_radius, bottom_radius, convex_radius;
GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius);
switch (inMode)
{
case ESupportMode::IncludeConvexRadius:
case ESupportMode::Default:
return new (&inBuffer) TaperedCylinder(top, bottom, top_radius, bottom_radius, 0.0f);
case ESupportMode::ExcludeConvexRadius:
return new (&inBuffer) TaperedCylinder(top - convex_radius, bottom + convex_radius, top_radius - convex_radius, bottom_radius - convex_radius, convex_radius);
}
JPH_ASSERT(false);
return nullptr;
}
JPH_INLINE static Vec3 sCalculateSideNormalXZ(Vec3Arg inSurfacePosition)
{
return (Vec3(1, 0, 1) * inSurfacePosition).NormalizedOr(Vec3::sAxisX());
}
JPH_INLINE static Vec3 sCalculateSideNormal(Vec3Arg inNormalXZ, float inTop, float inBottom, float inTopRadius, float inBottomRadius)
{
float tan_alpha = (inBottomRadius - inTopRadius) / (inTop - inBottom);
return Vec3(inNormalXZ.GetX(), tan_alpha, inNormalXZ.GetZ()).Normalized();
}
void TaperedCylinderShape::GetSupportingFace(const SubShapeID &inSubShapeID, Vec3Arg inDirection, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform, SupportingFace &outVertices) const
{
JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
JPH_ASSERT(IsValidScale(inScale));
// Get scaled tapered cylinder
float top, bottom, top_radius, bottom_radius, convex_radius;
GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius);
// Get the normal of the side of the cylinder
Vec3 normal_xz = sCalculateSideNormalXZ(-inDirection);
Vec3 normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius);
constexpr float cMinRadius = 1.0e-3f;
// Check if the normal is closer to the side than to the top or bottom
if (abs(normal.Dot(inDirection)) > abs(inDirection.GetY()))
{
// Return the side of the cylinder
outVertices.push_back(inCenterOfMassTransform * (normal_xz * top_radius + Vec3(0, top, 0)));
outVertices.push_back(inCenterOfMassTransform * (normal_xz * bottom_radius + Vec3(0, bottom, 0)));
}
else
{
// When the inDirection is more than 5 degrees from vertical, align the vertices so that 1 of the vertices
// points towards inDirection in the XZ plane. This ensures that we always have a vertex towards max penetration depth.
Mat44 transform = inCenterOfMassTransform;
Vec4 base_x = Vec4(inDirection.GetX(), 0, inDirection.GetZ(), 0);
float xz_sq = base_x.LengthSq();
float y_sq = Square(inDirection.GetY());
if (xz_sq > 0.00765427f * y_sq)
{
base_x /= sqrt(xz_sq);
Vec4 base_z = base_x.Swizzle<SWIZZLE_Z, SWIZZLE_Y, SWIZZLE_X, SWIZZLE_W>() * Vec4(-1, 0, 1, 0);
transform = transform * Mat44(base_x, Vec4(0, 1, 0, 0), base_z, Vec4(0, 0, 0, 1));
}
if (inDirection.GetY() < 0.0f)
{
// Top of the cylinder
if (top_radius > cMinRadius)
{
Vec3 top_3d(0, top, 0);
for (Vec3 v : cTaperedCylinderFace)
outVertices.push_back(transform * (top_radius * v + top_3d));
}
}
else
{
// Bottom of the cylinder
if (bottom_radius > cMinRadius)
{
Vec3 bottom_3d(0, bottom, 0);
for (const Vec3 *v = cTaperedCylinderFace + std::size(cTaperedCylinderFace) - 1; v >= cTaperedCylinderFace; --v)
outVertices.push_back(transform * (bottom_radius * *v + bottom_3d));
}
}
}
}
MassProperties TaperedCylinderShape::GetMassProperties() const
{
MassProperties p;
// Calculate mass
float density = GetDensity();
p.mMass = GetVolume() * density;
// Calculate inertia of a tapered cylinder (using wxMaxima)
// Radius:
// r(x):=br+(x-b)*(tr-br)/(t-b);
// Where t=top, b=bottom, tr=top radius, br=bottom radius
// Area of the cross section of the cylinder at x:
// area(x):=%pi*r(x)^2;
// Inertia x slice at x (using inertia of a solid disc, see https://en.wikipedia.org/wiki/List_of_moments_of_inertia, note needs to be multiplied by density):
// dix(x):=area(x)*r(x)^2/4;
// Inertia y slice at y (note needs to be multiplied by density)
// diy(x):=area(x)*r(x)^2/2;
// Volume:
// volume(b,t):=integrate(area(x),x,b,t);
// The constant density (note that we have this through GetDensity() so we'll use that instead):
// density(b,t):=m/volume(b,t);
// Inertia tensor element xx, note that we use the parallel axis theorem to move the inertia: Ixx' = Ixx + m translation^2, also note we multiply by density here:
// Ixx(br,tr,b,t):=integrate(dix(x)+area(x)*x^2,x,b,t)*density(b,t);
// Inertia tensor element yy:
// Iyy(br,tr,b,t):=integrate(diy(x),x,b,t)*density(b,t);
// Note that we can simplify Ixx by using:
// Ixx_delta(br,tr,b,t):=Ixx(br,tr,b,t)-Iyy(br,tr,b,t)/2;
// For a cylinder this formula matches what is listed on the wiki:
// factor(Ixx(r,r,-h/2,h/2));
// factor(Iyy(r,r,-h/2,h/2));
// For a cone with tip at origin too:
// factor(Ixx(0,r,0,h));
// factor(Iyy(0,r,0,h));
// Now for the tapered cylinder:
// rat(Ixx(br,tr,b,t),br,bt);
// rat(Iyy(br,tr,b,t),br,bt);
// rat(Ixx_delta(br,tr,b,t),br,bt);
float t = mTop;
float t2 = Square(t);
float t3 = t * t2;
float b = mBottom;
float b2 = Square(b);
float b3 = b * b2;
float br = mBottomRadius;
float br2 = Square(br);
float br3 = br * br2;
float br4 = Square(br2);
float tr = mTopRadius;
float tr2 = Square(tr);
float tr3 = tr * tr2;
float tr4 = Square(tr2);
float inertia_y = (JPH_PI / 10.0f) * density * (t - b) * (br4 + tr * br3 + tr2 * br2 + tr3 * br + tr4);
float inertia_x_delta = (JPH_PI / 30.0f) * density * ((t3 + 2 * b * t2 + 3 * b2 * t - 6 * b3) * br2 + (3 * t3 + b * t2 - b2 * t - 3 * b3) * tr * br + (6 * t3 - 3 * b * t2 - 2 * b2 * t - b3) * tr2);
float inertia_x = inertia_x_delta + inertia_y / 2;
float inertia_z = inertia_x;
p.mInertia = Mat44::sScale(Vec3(inertia_x, inertia_y, inertia_z));
return p;
}
Vec3 TaperedCylinderShape::GetSurfaceNormal(const SubShapeID &inSubShapeID, Vec3Arg inLocalSurfacePosition) const
{
JPH_ASSERT(inSubShapeID.IsEmpty(), "Invalid subshape ID");
constexpr float cEpsilon = 1.0e-5f;
if (inLocalSurfacePosition.GetY() > mTop - cEpsilon)
return Vec3(0, 1, 0);
else if (inLocalSurfacePosition.GetY() < mBottom + cEpsilon)
return Vec3(0, -1, 0);
else
return sCalculateSideNormal(sCalculateSideNormalXZ(inLocalSurfacePosition), mTop, mBottom, mTopRadius, mBottomRadius);
}
AABox TaperedCylinderShape::GetLocalBounds() const
{
float max_radius = max(mTopRadius, mBottomRadius);
return AABox(Vec3(-max_radius, mBottom, -max_radius), Vec3(max_radius, mTop, max_radius));
}
void TaperedCylinderShape::CollidePoint(Vec3Arg inPoint, const SubShapeIDCreator &inSubShapeIDCreator, CollidePointCollector &ioCollector, const ShapeFilter &inShapeFilter) const
{
// Test shape filter
if (!inShapeFilter.ShouldCollide(this, inSubShapeIDCreator.GetID()))
return;
// Check if the point is in the tapered cylinder
if (inPoint.GetY() >= mBottom && inPoint.GetY() <= mTop // Within height
&& Square(inPoint.GetX()) + Square(inPoint.GetZ()) <= Square(mBottomRadius + (inPoint.GetY() - mBottom) * (mTopRadius - mBottomRadius) / (mTop - mBottom))) // Within the radius
ioCollector.AddHit({ TransformedShape::sGetBodyID(ioCollector.GetContext()), inSubShapeIDCreator.GetID() });
}
void TaperedCylinderShape::CollideSoftBodyVertices(Mat44Arg inCenterOfMassTransform, Vec3Arg inScale, const CollideSoftBodyVertexIterator &inVertices, uint inNumVertices, int inCollidingShapeIndex) const
{
JPH_ASSERT(IsValidScale(inScale));
Mat44 inverse_transform = inCenterOfMassTransform.InversedRotationTranslation();
// Get scaled tapered cylinder
float top, bottom, top_radius, bottom_radius, convex_radius;
GetScaled(inScale, top, bottom, top_radius, bottom_radius, convex_radius);
Vec3 top_3d(0, top, 0);
Vec3 bottom_3d(0, bottom, 0);
for (CollideSoftBodyVertexIterator v = inVertices, sbv_end = inVertices + inNumVertices; v != sbv_end; ++v)
if (v.GetInvMass() > 0.0f)
{
Vec3 local_pos = inverse_transform * v.GetPosition();
// Calculate penetration into side surface
Vec3 normal_xz = sCalculateSideNormalXZ(local_pos);
Vec3 side_normal = sCalculateSideNormal(normal_xz, top, bottom, top_radius, bottom_radius);
Vec3 side_support_top = normal_xz * top_radius + top_3d;
float side_penetration = (side_support_top - local_pos).Dot(side_normal);
// Calculate penetration into top and bottom plane
float top_penetration = top - local_pos.GetY();
float bottom_penetration = local_pos.GetY() - bottom;
float min_top_bottom_penetration = min(top_penetration, bottom_penetration);
Vec3 point, normal;
if (side_penetration < 0.0f || min_top_bottom_penetration < 0.0f)
{
// We're outside the cylinder
// Calculate the closest point on the line segment from bottom to top support point:
// closest_point = bottom + fraction * (top - bottom) / |top - bottom|^2
Vec3 side_support_bottom = normal_xz * bottom_radius + bottom_3d;
Vec3 bottom_to_top = side_support_top - side_support_bottom;
float fraction = (local_pos - side_support_bottom).Dot(bottom_to_top);
// Calculate the distance to the axis of the cylinder
float distance_to_axis = normal_xz.Dot(local_pos);
bool inside_top_radius = distance_to_axis <= top_radius;
bool inside_bottom_radius = distance_to_axis <= bottom_radius;
/*
Regions of tapered cylinder (side view):
_ B | |
--_ | A |
t-------+
C / \
/ tapered \
_ / cylinder \
--_ / \
b-----------------+
D | E |
| |
t = side_support_top, b = side_support_bottom
Lines between B and C and C and D are at a 90 degree angle to the line between t and b
*/
if (fraction >= bottom_to_top.LengthSq() // Region B: Above the line segment
&& !inside_top_radius) // Outside the top radius
{
// Top support point is closest
point = side_support_top;
normal = (local_pos - point).NormalizedOr(Vec3::sAxisY());
}
else if (fraction < 0.0f // Region D: Below the line segment
&& !inside_bottom_radius) // Outside the bottom radius
{
// Bottom support point is closest
point = side_support_bottom;
normal = (local_pos - point).NormalizedOr(Vec3::sAxisY());
}
else if (top_penetration < 0.0f // Region A: Above the top plane
&& inside_top_radius) // Inside the top radius
{
// Top plane is closest
point = top_3d;
normal = Vec3(0, 1, 0);
}
else if (bottom_penetration < 0.0f // Region E: Below the bottom plane
&& inside_bottom_radius) // Inside the bottom radius
{
// Bottom plane is closest
point = bottom_3d;
normal = Vec3(0, -1, 0);
}
else // Region C
{
// Side surface is closest
point = side_support_top;
normal = side_normal;
}
}
else if (side_penetration < min_top_bottom_penetration)
{
// Side surface is closest
point = side_support_top;
normal = side_normal;
}
else if (top_penetration < bottom_penetration)
{
// Top plane is closest
point = top_3d;
normal = Vec3(0, 1, 0);
}
else
{
// Bottom plane is closest
point = bottom_3d;
normal = Vec3(0, -1, 0);
}
// Calculate penetration
Plane plane = Plane::sFromPointAndNormal(point, normal);
float penetration = -plane.SignedDistance(local_pos);
if (v.UpdatePenetration(penetration))
v.SetCollision(plane.GetTransformed(inCenterOfMassTransform), inCollidingShapeIndex);
}
}
class TaperedCylinderShape::TCSGetTrianglesContext
{
public:
explicit TCSGetTrianglesContext(Mat44Arg inTransform) : mTransform(inTransform) { }
Mat44 mTransform;
uint mProcessed = 0; // Which elements we processed, bit 0 = top, bit 1 = bottom, bit 2 = side
};
void TaperedCylinderShape::GetTrianglesStart(GetTrianglesContext &ioContext, const AABox &inBox, Vec3Arg inPositionCOM, QuatArg inRotation, Vec3Arg inScale) const
{
static_assert(sizeof(TCSGetTrianglesContext) <= sizeof(GetTrianglesContext), "GetTrianglesContext too small");
JPH_ASSERT(IsAligned(&ioContext, alignof(TCSGetTrianglesContext)));
// Make sure the scale is not inside out
Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? inScale.FlipSign<-1, 1, 1>() : inScale;
// Mark top and bottom processed if their radius is too small
TCSGetTrianglesContext *context = new (&ioContext) TCSGetTrianglesContext(Mat44::sRotationTranslation(inRotation, inPositionCOM) * Mat44::sScale(scale));
constexpr float cMinRadius = 1.0e-3f;
if (mTopRadius < cMinRadius)
context->mProcessed |= 0b001;
if (mBottomRadius < cMinRadius)
context->mProcessed |= 0b010;
}
int TaperedCylinderShape::GetTrianglesNext(GetTrianglesContext &ioContext, int inMaxTrianglesRequested, Float3 *outTriangleVertices, const PhysicsMaterial **outMaterials) const
{
constexpr int cNumVertices = int(std::size(cTaperedCylinderFace));
static_assert(cGetTrianglesMinTrianglesRequested >= 2 * cNumVertices);
JPH_ASSERT(inMaxTrianglesRequested >= cGetTrianglesMinTrianglesRequested);
TCSGetTrianglesContext &context = (TCSGetTrianglesContext &)ioContext;
int total_num_triangles = 0;
// Top cap
Vec3 top_3d(0, mTop, 0);
if ((context.mProcessed & 0b001) == 0)
{
Vec3 v0 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[0]);
Vec3 v1 = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[1]);
for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
{
Vec3 v2 = context.mTransform * (top_3d + mTopRadius * *v);
v0.StoreFloat3(outTriangleVertices++);
v1.StoreFloat3(outTriangleVertices++);
v2.StoreFloat3(outTriangleVertices++);
v1 = v2;
}
total_num_triangles = cNumVertices - 2;
context.mProcessed |= 0b001;
}
// Bottom cap
Vec3 bottom_3d(0, mBottom, 0);
if ((context.mProcessed & 0b010) == 0
&& total_num_triangles + cNumVertices - 2 < inMaxTrianglesRequested)
{
Vec3 v0 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[0]);
Vec3 v1 = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[1]);
for (const Vec3 *v = cTaperedCylinderFace + 2, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
{
Vec3 v2 = context.mTransform * (bottom_3d + mBottomRadius * *v);
v0.StoreFloat3(outTriangleVertices++);
v2.StoreFloat3(outTriangleVertices++);
v1.StoreFloat3(outTriangleVertices++);
v1 = v2;
}
total_num_triangles += cNumVertices - 2;
context.mProcessed |= 0b010;
}
// Side
if ((context.mProcessed & 0b100) == 0
&& total_num_triangles + 2 * cNumVertices < inMaxTrianglesRequested)
{
Vec3 v0t = context.mTransform * (top_3d + mTopRadius * cTaperedCylinderFace[cNumVertices - 1]);
Vec3 v0b = context.mTransform * (bottom_3d + mBottomRadius * cTaperedCylinderFace[cNumVertices - 1]);
for (const Vec3 *v = cTaperedCylinderFace, *v_end = cTaperedCylinderFace + cNumVertices; v < v_end; ++v)
{
Vec3 v1t = context.mTransform * (top_3d + mTopRadius * *v);
v0t.StoreFloat3(outTriangleVertices++);
v0b.StoreFloat3(outTriangleVertices++);
v1t.StoreFloat3(outTriangleVertices++);
Vec3 v1b = context.mTransform * (bottom_3d + mBottomRadius * *v);
v1t.StoreFloat3(outTriangleVertices++);
v0b.StoreFloat3(outTriangleVertices++);
v1b.StoreFloat3(outTriangleVertices++);
v0t = v1t;
v0b = v1b;
}
total_num_triangles += 2 * cNumVertices;
context.mProcessed |= 0b100;
}
// Store materials
if (outMaterials != nullptr)
{
const PhysicsMaterial *material = GetMaterial();
for (const PhysicsMaterial **m = outMaterials, **m_end = outMaterials + total_num_triangles; m < m_end; ++m)
*m = material;
}
return total_num_triangles;
}
#ifdef JPH_DEBUG_RENDERER
void TaperedCylinderShape::Draw(DebugRenderer *inRenderer, RMat44Arg inCenterOfMassTransform, Vec3Arg inScale, ColorArg inColor, bool inUseMaterialColors, bool inDrawWireframe) const
{
// Preserve flip along y axis but make sure we're not inside out
Vec3 scale = ScaleHelpers::IsInsideOut(inScale)? inScale.FlipSign<-1, 1, 1>() : inScale;
RMat44 world_transform = inCenterOfMassTransform * Mat44::sScale(scale);
DebugRenderer::EDrawMode draw_mode = inDrawWireframe? DebugRenderer::EDrawMode::Wireframe : DebugRenderer::EDrawMode::Solid;
inRenderer->DrawTaperedCylinder(world_transform, mTop, mBottom, mTopRadius, mBottomRadius, inUseMaterialColors? GetMaterial()->GetDebugColor() : inColor, DebugRenderer::ECastShadow::On, draw_mode);
}
#endif // JPH_DEBUG_RENDERER
void TaperedCylinderShape::SaveBinaryState(StreamOut &inStream) const
{
ConvexShape::SaveBinaryState(inStream);
inStream.Write(mTop);
inStream.Write(mBottom);
inStream.Write(mTopRadius);
inStream.Write(mBottomRadius);
inStream.Write(mConvexRadius);
}
void TaperedCylinderShape::RestoreBinaryState(StreamIn &inStream)
{
ConvexShape::RestoreBinaryState(inStream);
inStream.Read(mTop);
inStream.Read(mBottom);
inStream.Read(mTopRadius);
inStream.Read(mBottomRadius);
inStream.Read(mConvexRadius);
}
float TaperedCylinderShape::GetVolume() const
{
// Volume of a tapered cylinder is: integrate(%pi*(b+x*(t-b)/h)^2,x,0,h) where t is the top radius, b is the bottom radius and h is the height
return (JPH_PI / 3.0f) * (mTop - mBottom) * (Square(mTopRadius) + mTopRadius * mBottomRadius + Square(mBottomRadius));
}
bool TaperedCylinderShape::IsValidScale(Vec3Arg inScale) const
{
return ConvexShape::IsValidScale(inScale) && ScaleHelpers::IsUniformScaleXZ(inScale.Abs());
}
Vec3 TaperedCylinderShape::MakeScaleValid(Vec3Arg inScale) const
{
Vec3 scale = ScaleHelpers::MakeNonZeroScale(inScale);
return scale.GetSign() * ScaleHelpers::MakeUniformScaleXZ(scale.Abs());
}
void TaperedCylinderShape::sRegister()
{
ShapeFunctions &f = ShapeFunctions::sGet(EShapeSubType::TaperedCylinder);
f.mConstruct = []() -> Shape * { return new TaperedCylinderShape; };
f.mColor = Color::sGreen;
}
JPH_NAMESPACE_END