654 lines
22 KiB
C++
654 lines
22 KiB
C++
// Redistribution and use in source and binary forms, with or without
|
|
// modification, are permitted provided that the following conditions
|
|
// are met:
|
|
// * Redistributions of source code must retain the above copyright
|
|
// notice, this list of conditions and the following disclaimer.
|
|
// * Redistributions in binary form must reproduce the above copyright
|
|
// notice, this list of conditions and the following disclaimer in the
|
|
// documentation and/or other materials provided with the distribution.
|
|
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
|
// contributors may be used to endorse or promote products derived
|
|
// from this software without specific prior written permission.
|
|
//
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
|
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
|
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
|
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
|
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
|
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
|
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
//
|
|
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
|
|
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
|
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
|
|
|
#include "foundation/PxBasicTemplates.h"
|
|
#include "geometry/PxConvexCoreGeometry.h"
|
|
#include "geometry/PxSphereGeometry.h"
|
|
#include "geometry/PxCapsuleGeometry.h"
|
|
#include "geometry/PxBoxGeometry.h"
|
|
#include "geometry/PxConvexMeshGeometry.h"
|
|
#include "GuConvexGeometry.h"
|
|
#include "GuConvexSupport.h"
|
|
#include "GuBounds.h"
|
|
#include "common/PxRenderOutput.h"
|
|
|
|
using namespace physx;
|
|
|
|
// This enums should match
|
|
PX_COMPILE_TIME_ASSERT(PxU32(Gu::ConvexCore::Type::ePOINT) == PxU32(PxConvexCore::ePOINT));
|
|
PX_COMPILE_TIME_ASSERT(PxU32(Gu::ConvexCore::Type::eSEGMENT) == PxU32(PxConvexCore::eSEGMENT));
|
|
PX_COMPILE_TIME_ASSERT(PxU32(Gu::ConvexCore::Type::eBOX) == PxU32(PxConvexCore::eBOX));
|
|
PX_COMPILE_TIME_ASSERT(PxU32(Gu::ConvexCore::Type::eELLIPSOID) == PxU32(PxConvexCore::eELLIPSOID));
|
|
PX_COMPILE_TIME_ASSERT(PxU32(Gu::ConvexCore::Type::eCYLINDER) == PxU32(PxConvexCore::eCYLINDER));
|
|
|
|
PX_COMPILE_TIME_ASSERT(Gu::ConvexCore::MAX_CORE_SIZE >= PxConvexCoreGeometry::MAX_CORE_SIZE);
|
|
|
|
static const PxU32 GEOMETRY_COLOR = PxU32(PxDebugColor::eARGB_MAGENTA);
|
|
static const PxU32 GEOMETRY_CORE_COLOR = PxU32(0x88880088); // dark magenta
|
|
|
|
namespace aux
|
|
{
|
|
PxReal coneRadiusAtHeight(PxReal height, PxReal radius, PxReal margin, PxReal h)
|
|
{
|
|
float angle = atan2f(radius, height);
|
|
float aSin = sinf(angle);
|
|
float aCos = cosf(angle);
|
|
|
|
if (h > -height * 0.5f + margin * aSin && h < height * 0.5f + margin * aSin)
|
|
return radius * (height * 0.5f - h) / height + margin / aCos;
|
|
|
|
if (h <= -height * 0.5f + margin * aSin)
|
|
{
|
|
float a = -h - height * 0.5f;
|
|
return radius + sqrtf(margin * margin - a * a);
|
|
}
|
|
|
|
if (h >= height * 0.5f + margin * aSin)
|
|
{
|
|
float a = h - height * 0.5f;
|
|
return sqrtf(margin * margin - a * a);
|
|
}
|
|
|
|
PX_ASSERT(0);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
namespace debug
|
|
{
|
|
static void drawArc(const PxVec3& center, const PxVec3& radius, const PxVec3& axis, PxReal angle, PxReal error, PxRenderOutput& out)
|
|
{
|
|
int sides = int(ceilf(angle / (2 * acosf(1.0f - error))));
|
|
float step = angle / sides;
|
|
out << PxRenderOutput::LINESTRIP;
|
|
for (int i = 0; i <= sides; ++i)
|
|
out << center + PxQuat(step * i, axis).rotate(radius);
|
|
}
|
|
static void drawCircle(const PxVec3& center, const PxVec3& radius, const PxVec3& axis, PxReal error, PxRenderOutput& out)
|
|
{
|
|
drawArc(center, radius, axis, PxTwoPi, error, out);
|
|
}
|
|
static void drawQuarterCircle(const PxVec3& center, const PxVec3& radius, const PxVec3& axis, PxReal error, PxRenderOutput& out)
|
|
{
|
|
drawArc(center, radius, axis, PxPiDivTwo, error, out);
|
|
}
|
|
static void drawLine(const PxVec3& s, const PxVec3& e, PxRenderOutput& out)
|
|
{
|
|
out << PxRenderOutput::LINES << s << e;
|
|
}
|
|
static void drawSphere(PxReal radius, PxReal error, PxRenderOutput& out)
|
|
{
|
|
drawCircle(PxVec3(0), PxVec3(0, radius, 0), PxVec3(1, 0, 0), error, out);
|
|
drawCircle(PxVec3(0), PxVec3(0, 0, radius), PxVec3(0, 1, 0), error, out);
|
|
drawCircle(PxVec3(0), PxVec3(radius, 0, 0), PxVec3(0, 0, 1), error, out);
|
|
}
|
|
static void drawCapsule(PxReal length, PxReal radius, PxReal error, PxRenderOutput& out)
|
|
{
|
|
drawLine(PxVec3(length * 0.5f, radius, 0), PxVec3(-length * 0.5f, radius, 0), out);
|
|
drawLine(PxVec3(length * 0.5f, -radius, 0), PxVec3(-length * 0.5f, -radius, 0), out);
|
|
drawLine(PxVec3(length * 0.5f, 0, radius), PxVec3(-length * 0.5f, 0, radius), out);
|
|
drawLine(PxVec3(length * 0.5f, 0, -radius), PxVec3(-length * 0.5f, 0, -radius), out);
|
|
drawCircle(PxVec3(length * 0.5f, 0, 0), PxVec3(0, radius, 0), PxVec3(1, 0, 0), error, out);
|
|
drawCircle(PxVec3(-length * 0.5f, 0, 0), PxVec3(0, radius, 0), PxVec3(1, 0, 0), error, out);
|
|
drawArc(PxVec3(length * 0.5f, 0, 0), PxVec3(0, radius, 0), PxVec3(0, 0, -1), PxPi, error, out);
|
|
drawArc(PxVec3(length * 0.5f, 0, 0), PxVec3(0, 0, radius), PxVec3(0, 1, 0), PxPi, error, out);
|
|
drawArc(PxVec3(-length * 0.5f, 0, 0), PxVec3(0, radius, 0), PxVec3(0, 0, 1), PxPi, error, out);
|
|
drawArc(PxVec3(-length * 0.5f, 0, 0), PxVec3(0, 0, radius), PxVec3(0, -1, 0), PxPi, error, out);
|
|
}
|
|
static void drawBox(const PxVec3& extents, PxReal margin, PxReal error, PxRenderOutput& out)
|
|
{
|
|
for (PxU32 i = 0; i < 3; ++i)
|
|
{
|
|
PxU32 axis0 = i, axis1 = (i + 1) % 3, axis2 = (i + 2) % 3;
|
|
PxVec3 ax0(0), ax1(0), ax2(0);
|
|
ax0[axis0] = 1; ax1[axis1] = 1; ax2[axis2] = 1;
|
|
PxReal s[4][2] = { { 1, 1 }, { -1, 1 }, { -1,-1 }, { 1, -1 } };
|
|
for (PxU32 j = 0; j < 4; ++j)
|
|
{
|
|
PxVec3 c(0);
|
|
c[axis1] = extents[axis1] * 0.5f * s[j][0];
|
|
c[axis2] = extents[axis2] * 0.5f * s[j][1];
|
|
if (margin > FLT_EPSILON)
|
|
{
|
|
drawLine(c + ax0 * extents[axis0] * 0.5f + ax1 * margin * s[j][0], c - ax0 * extents[axis0] * 0.5f + ax1 * margin * s[j][0], out);
|
|
drawLine(c + ax0 * extents[axis0] * 0.5f + ax2 * margin * s[j][1], c - ax0 * extents[axis0] * 0.5f + ax2 * margin * s[j][1], out);
|
|
drawQuarterCircle(c + ax0 * extents[axis0] * 0.5f, ax1 * margin * s[j][0], ax0 * (j % 2 ? -1.0f : 1.0f), error, out);
|
|
drawQuarterCircle(c - ax0 * extents[axis0] * 0.5f, ax1 * margin * s[j][0], ax0 * (j % 2 ? -1.0f : 1.0f), error, out);
|
|
}
|
|
else
|
|
drawLine(c + ax0 * extents[axis0] * 0.5f, c - ax0 * extents[axis0] * 0.5f, out);
|
|
}
|
|
}
|
|
}
|
|
static void drawEllipse(const PxVec3& center, const PxVec3& axis0, PxReal radius0, const PxVec3& axis1, PxReal radius1, PxReal margin, PxReal error, PxRenderOutput& out)
|
|
{
|
|
if (radius0 * radius1 > 0)
|
|
{
|
|
out << PxRenderOutput::LINESTRIP;
|
|
for (PxReal t = 0, dT = 0; t - dT < PxTwoPi; t += dT)
|
|
{
|
|
PxReal si, co; PxSinCos(t, si, co);
|
|
PxVec3 p = axis0 * radius0 * co + axis1 * radius1 * si;
|
|
PxVec3 tang = -axis0 * radius0 * si + axis1 * radius1 * co;
|
|
PxVec3 norm = axis1.cross(axis0).cross(tang).getNormalized();
|
|
p += norm * margin;
|
|
out << center + p;
|
|
PxReal d0, d1, d20, d21, C;
|
|
d0 = -radius0 * si; d1 = radius1 * co; d20 = -radius0 * co; d21 = -radius1 * si;
|
|
C = PxAbs(d0 * d21 - d1 * d20) / PxPow(d0 * d0 + d1 * d1, 1.5f);
|
|
dT = 100.0f * error / (1.0f + C) / PxMax(radius0, radius1);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
const PxVec3 axis = axis1.cross(axis0);
|
|
if (radius0 > 0)
|
|
{
|
|
drawLine(center + axis0 * radius0 + axis1 * margin, center - axis0 * radius0 + axis1 * margin, out);
|
|
drawLine(center + axis0 * radius0 - axis1 * margin, center - axis0 * radius0 - axis1 * margin, out);
|
|
drawArc(center + axis0 * radius0, axis1 * margin, axis, PxPi, error, out);
|
|
drawArc(center - axis0 * radius0, axis1 * margin, -axis, PxPi, error, out);
|
|
}
|
|
else if (radius1 > 0)
|
|
{
|
|
drawLine(center + axis1 * radius1 + axis0 * margin, center - axis1 * radius1 + axis0 * margin, out);
|
|
drawLine(center + axis1 * radius1 - axis0 * margin, center - axis1 * radius1 - axis0 * margin, out);
|
|
drawArc(center + axis1 * radius1, axis0 * margin, -axis, PxPi, error, out);
|
|
drawArc(center - axis1 * radius1, axis0 * margin, axis, PxPi, error, out);
|
|
}
|
|
else
|
|
{
|
|
drawArc(center - axis1 * radius1, axis0 * margin, axis, PxTwoPi, error, out);
|
|
}
|
|
}
|
|
}
|
|
static void drawEllipsoid(const PxVec3& radii, PxReal margin, PxReal error, PxRenderOutput& out)
|
|
{
|
|
const PxVec3 zero(0), X(1, 0, 0), Y(0, 1, 0), Z(0, 0, 1);
|
|
drawEllipse(zero, X, radii.x, Y, radii.y, margin, error, out);
|
|
drawEllipse(zero, X, radii.x, Z, radii.z, margin, error, out);
|
|
drawEllipse(zero, Z, radii.z, Y, radii.y, margin, error, out);
|
|
}
|
|
static void drawCylinder(PxReal height, PxReal radius, PxReal margin, PxReal error, PxRenderOutput& out)
|
|
{
|
|
const PxReal ERR = error;
|
|
|
|
PxU32 axis0 = 0, axis1 = 1, axis2 = 2;
|
|
|
|
PxVec3 zr(PxZero), rd(PxZero), ax(PxZero), ax1(PxZero), ax2(PxZero), r0(PxZero), r1(PxZero);
|
|
ax[axis0] = ax1[axis1] = ax2[axis2] = 1.0f;
|
|
r0[axis1] = r1[axis2] = radius;
|
|
|
|
rd[axis1] = radius;
|
|
rd[axis0] = -(height * 0.5f + margin);
|
|
debug::drawCircle(zr, rd, ax, ERR, out);
|
|
rd[axis0] = (height * 0.5f + margin);
|
|
debug::drawCircle(zr, rd, ax, ERR, out);
|
|
rd[axis1] = radius + margin;
|
|
rd[axis0] = -(height * 0.5f);
|
|
debug::drawCircle(zr, rd, ax, ERR, out);
|
|
rd[axis0] = (height * 0.5f);
|
|
debug::drawCircle(zr, rd, ax, ERR, out);
|
|
|
|
debug::drawLine(-ax * height * 0.5f + ax1 * (radius + margin), ax * height * 0.5f + ax1 * (radius + margin), out);
|
|
debug::drawLine(-ax * height * 0.5f - ax1 * (radius + margin), ax * height * 0.5f - ax1 * (radius + margin), out);
|
|
debug::drawLine(-ax * height * 0.5f + ax2 * (radius + margin), ax * height * 0.5f + ax2 * (radius + margin), out);
|
|
debug::drawLine(-ax * height * 0.5f - ax2 * (radius + margin), ax * height * 0.5f - ax2 * (radius + margin), out);
|
|
|
|
debug::drawQuarterCircle(-ax * height * 0.5f + ax1 * radius, -ax * margin, -ax2, ERR, out);
|
|
debug::drawQuarterCircle(-ax * height * 0.5f - ax1 * radius, -ax * margin, ax2, ERR, out);
|
|
debug::drawQuarterCircle(-ax * height * 0.5f + ax2 * radius, -ax * margin, ax1, ERR, out);
|
|
debug::drawQuarterCircle(-ax * height * 0.5f - ax2 * radius, -ax * margin, -ax1, ERR, out);
|
|
|
|
debug::drawQuarterCircle(ax * height * 0.5f + ax1 * radius, ax * margin, ax2, ERR, out);
|
|
debug::drawQuarterCircle(ax * height * 0.5f - ax1 * radius, ax * margin, -ax2, ERR, out);
|
|
debug::drawQuarterCircle(ax * height * 0.5f + ax2 * radius, ax * margin, -ax1, ERR, out);
|
|
debug::drawQuarterCircle(ax * height * 0.5f - ax2 * radius, ax * margin, ax1, ERR, out);
|
|
}
|
|
static void drawCone(PxReal height, PxReal radius, PxReal margin, PxReal error, PxRenderOutput& out)
|
|
{
|
|
const PxReal ERR = error;
|
|
|
|
PxU32 axis0 = 0, axis1 = 1, axis2 = 2;
|
|
|
|
PxVec3 zr(PxZero), rd(PxZero), ax(PxZero), ax1(PxZero), ax2(PxZero), r0(PxZero), r1(PxZero);
|
|
ax[axis0] = ax1[axis1] = ax2[axis2] = 1.0f;
|
|
r0[axis1] = r1[axis2] = radius;
|
|
|
|
float ang = atan2f(radius, height);
|
|
float aSin = sinf(ang);
|
|
|
|
rd[axis1] = radius;
|
|
rd[axis0] = -(height * 0.5f + margin);
|
|
drawCircle(zr, rd, ax, ERR, out);
|
|
if (radius >= height && margin > 0)
|
|
{
|
|
rd[axis1] = radius + margin;
|
|
rd[axis0] = -height * 0.5f;
|
|
drawCircle(zr, rd, ax, ERR, out);
|
|
}
|
|
rd[axis0] = -(height * 0.5f) + margin * aSin;
|
|
rd[axis1] = aux::coneRadiusAtHeight(height, radius, margin, rd[axis0]);
|
|
drawCircle(zr, rd, ax, ERR, out);
|
|
rd[axis0] = height * 0.5f + margin * aSin;
|
|
rd[axis1] = aux::coneRadiusAtHeight(height, radius, margin, rd[axis0]);
|
|
drawCircle(zr, rd, ax, ERR, out);
|
|
|
|
float h0 = -height * 0.5f + margin * aSin, h1 = height * 0.5f + margin * aSin;
|
|
float s0 = aux::coneRadiusAtHeight(height, radius, margin, h0), s1 = aux::coneRadiusAtHeight(height, radius, margin, h1);
|
|
debug::drawLine(ax * h0 + ax1 * s0, ax * h1 + ax1 * s1, out);
|
|
debug::drawLine(ax * h0 - ax1 * s0, ax * h1 - ax1 * s1, out);
|
|
debug::drawLine(ax * h0 + ax2 * s0, ax * h1 + ax2 * s1, out);
|
|
debug::drawLine(ax * h0 - ax2 * s0, ax * h1 - ax2 * s1, out);
|
|
|
|
debug::drawArc(-ax * height * 0.5f + ax1 * radius, -ax * margin, -ax2, PxPiDivTwo + ang, ERR, out);
|
|
debug::drawArc(-ax * height * 0.5f - ax1 * radius, -ax * margin, ax2, PxPiDivTwo + ang, ERR, out);
|
|
debug::drawArc(-ax * height * 0.5f + ax2 * radius, -ax * margin, ax1, PxPiDivTwo + ang, ERR, out);
|
|
debug::drawArc(-ax * height * 0.5f - ax2 * radius, -ax * margin, -ax1, PxPiDivTwo + ang, ERR, out);
|
|
|
|
debug::drawArc(ax * height * 0.5f, ax * margin, ax2, PxPiDivTwo - ang, ERR, out);
|
|
debug::drawArc(ax * height * 0.5f, ax * margin, -ax2, PxPiDivTwo - ang, ERR, out);
|
|
debug::drawArc(ax * height * 0.5f, ax * margin, -ax1, PxPiDivTwo - ang, ERR, out);
|
|
debug::drawArc(ax * height * 0.5f, ax * margin, ax1, PxPiDivTwo - ang, ERR, out);
|
|
}
|
|
}
|
|
|
|
PX_PHYSX_COMMON_API bool PxConvexCoreGeometry::isValid() const
|
|
{
|
|
const PxReal margin = getMargin();
|
|
|
|
if (margin < 0)
|
|
return false;
|
|
|
|
switch (getCoreType())
|
|
{
|
|
case PxConvexCore::ePOINT:
|
|
return margin > 0; // has volume
|
|
|
|
case PxConvexCore::eSEGMENT:
|
|
{
|
|
const PxConvexCore::Segment& c = getCore<PxConvexCore::Segment>();
|
|
return c.length >= 0 &&
|
|
(margin + c.length) * margin > 0; // has volume
|
|
}
|
|
|
|
case PxConvexCore::eBOX:
|
|
{
|
|
const PxConvexCore::Box& c = getCore<PxConvexCore::Box>();
|
|
return c.extents.x >= 0 && c.extents.y >= 0 && c.extents.z >= 0 &&
|
|
(margin + c.extents.x) * (margin + c.extents.y) * (margin + c.extents.z) > 0; // has volume
|
|
}
|
|
|
|
case PxConvexCore::eELLIPSOID:
|
|
{
|
|
const PxConvexCore::Ellipsoid& c = getCore<PxConvexCore::Ellipsoid>();
|
|
return c.radii.x >= 0 && c.radii.y >= 0 && c.radii.z >= 0 &&
|
|
(margin + c.radii.x) * (margin + c.radii.y) * (margin + c.radii.z) > 0; // has volume
|
|
}
|
|
|
|
case PxConvexCore::eCYLINDER:
|
|
{
|
|
const PxConvexCore::Cylinder& c = getCore<PxConvexCore::Cylinder>();
|
|
return c.height >= 0 && c.radius >= 0 &&
|
|
(margin + c.height) * (margin + c.radius) > 0; // has volume
|
|
}
|
|
|
|
case PxConvexCore::eCONE:
|
|
{
|
|
const PxConvexCore::Cone& c = getCore<PxConvexCore::Cone>();
|
|
return c.height >= 0 && c.radius >= 0 &&
|
|
(margin + c.height) * (margin + c.radius) > 0; // has volume
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
PX_PHYSX_COMMON_API bool Gu::isGPUCompatible(const PxConvexCoreGeometry& convex)
|
|
{
|
|
// there's no types a.t.m. that don't support GPU,
|
|
// but if there will be, we'll return 'false' here.
|
|
switch (convex.getCoreType())
|
|
{
|
|
case PxConvexCore::ePOINT:
|
|
case PxConvexCore::eSEGMENT:
|
|
case PxConvexCore::eBOX:
|
|
case PxConvexCore::eELLIPSOID:
|
|
case PxConvexCore::eCYLINDER:
|
|
case PxConvexCore::eCONE:
|
|
return true;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
namespace
|
|
{
|
|
struct MassInfo
|
|
{
|
|
PxReal density1Mass;
|
|
PxMat33 inertiaTensor;
|
|
PxVec3 centerOfMass;
|
|
};
|
|
|
|
MassInfo cylinderMassInfo(PxReal height, PxReal radius, const PxVec3& centerOfMass = PxVec3(0))
|
|
{
|
|
const PxReal h = height, r = radius;
|
|
const PxReal m = PxPi * r * r * h;
|
|
const PxReal ix = m * r * r / 2;
|
|
const PxReal iyz = m * (3 * r * r + h * h) / 12;
|
|
MassInfo mi;
|
|
mi.density1Mass = m;
|
|
mi.inertiaTensor = PxMat33::createDiagonal(PxVec3(ix, iyz, iyz));
|
|
mi.centerOfMass = centerOfMass;
|
|
return mi;
|
|
}
|
|
}
|
|
|
|
PX_PHYSX_COMMON_API void Gu::computeMassInfo(const PxConvexCoreGeometry& convex, PxReal& density1Mass, PxMat33& inertiaTensor, PxVec3& centerOfMass)
|
|
{
|
|
PxReal margin = convex.getMargin();
|
|
switch (convex.getCoreType())
|
|
{
|
|
case PxConvexCore::ePOINT:
|
|
{
|
|
const PxReal r = margin;
|
|
density1Mass = PxPi * r * r * r * 4.0f / 3.0f;
|
|
inertiaTensor = PxMat33::createDiagonal(PxVec3(r * r)) * (density1Mass * 2.0f / 5.0f);
|
|
centerOfMass = PxVec3(0);
|
|
break;
|
|
}
|
|
|
|
case PxConvexCore::eCYLINDER:
|
|
{
|
|
const PxConvexCore::Cylinder& core = convex.getCore<PxConvexCore::Cylinder>();
|
|
const PxReal H = core.height + margin * 2;
|
|
const PxReal R = core.radius + margin;
|
|
MassInfo mi = cylinderMassInfo(H, R);
|
|
density1Mass = mi.density1Mass;
|
|
inertiaTensor = mi.inertiaTensor;
|
|
centerOfMass = mi.centerOfMass;
|
|
break;
|
|
}
|
|
|
|
case PxConvexCore::eCONE:
|
|
{
|
|
const PxU32 SLICE_COUNT = 32;
|
|
const PxConvexCore::Cone& core = convex.getCore<PxConvexCore::Cone>();
|
|
const PxReal H = core.height + margin * 2;
|
|
const PxReal h = H / SLICE_COUNT;
|
|
|
|
MassInfo mis[SLICE_COUNT];
|
|
|
|
for (PxU32 i = 0; i < SLICE_COUNT; ++i)
|
|
{
|
|
const PxReal t = -H * 0.5f + i * h + h * 0.5f;
|
|
const PxReal r = aux::coneRadiusAtHeight(core.height, core.radius, margin, t);
|
|
mis[i] = cylinderMassInfo(h, r, PxVec3(t, 0, 0));
|
|
}
|
|
|
|
MassInfo mi{ 0, PxMat33(PxZero), PxVec3(0) };
|
|
|
|
for (PxU32 i = 0; i < SLICE_COUNT; i++)
|
|
{
|
|
mi.density1Mass += mis[i].density1Mass;
|
|
mi.centerOfMass += mis[i].centerOfMass * mis[i].density1Mass;
|
|
}
|
|
|
|
if (mi.density1Mass > 0.f)
|
|
mi.centerOfMass /= mi.density1Mass;
|
|
|
|
for (PxU32 i = 0; i < SLICE_COUNT; i++)
|
|
{
|
|
const PxVec3 t = mi.centerOfMass - mis[i].centerOfMass;
|
|
const PxMat33 s(PxVec3(0, t.z, -t.y), PxVec3(-t.z, 0, t.x), PxVec3(t.y, -t.x, 0));
|
|
mi.inertiaTensor += s.getTranspose() * s * mis[i].density1Mass + mis[i].inertiaTensor;
|
|
}
|
|
|
|
{
|
|
const PxVec3 t = mi.centerOfMass;
|
|
const PxMat33 s(PxVec3(0, t.z, -t.y), PxVec3(-t.z, 0, t.x), PxVec3(t.y, -t.x, 0));
|
|
mi.inertiaTensor += s.getTranspose() * s * mi.density1Mass;
|
|
}
|
|
|
|
density1Mass = mi.density1Mass;
|
|
inertiaTensor = mi.inertiaTensor;
|
|
centerOfMass = mi.centerOfMass;
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
const PxBounds3 bounds = Gu::computeBounds(convex, PxTransform(PxIdentity));
|
|
const PxVec3 halfExtents = bounds.getDimensions() * 0.5f;
|
|
density1Mass = halfExtents.x * halfExtents.y * halfExtents.z * 8.0f;
|
|
PxVec3 d2 = halfExtents.multiply(halfExtents);
|
|
inertiaTensor = PxMat33::createDiagonal(PxVec3(d2.y + d2.z, d2.x + d2.z, d2.x + d2.y)) * (density1Mass * 1.0f / 3.0f);
|
|
centerOfMass = bounds.getCenter();
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
PX_PHYSX_COMMON_API void Gu::visualize(const PxConvexCoreGeometry& convex, const PxTransform& pose, bool drawCore, const PxBounds3& cullbox, PxRenderOutput& out)
|
|
{
|
|
PX_UNUSED(cullbox);
|
|
|
|
PxReal margin = convex.getMargin();
|
|
|
|
switch (convex.getCoreType())
|
|
{
|
|
case PxConvexCore::ePOINT:
|
|
{
|
|
const PxReal error = 0.001f;
|
|
|
|
out << pose;
|
|
out << GEOMETRY_COLOR;
|
|
debug::drawSphere(margin, error, out);
|
|
|
|
if (drawCore)
|
|
{
|
|
out << GEOMETRY_CORE_COLOR;
|
|
debug::drawSphere(0, error, out);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case PxConvexCore::eSEGMENT:
|
|
{
|
|
const PxConvexCore::Segment& core = convex.getCore<PxConvexCore::Segment>();
|
|
const PxReal error = 0.001f;
|
|
|
|
out << pose;
|
|
out << GEOMETRY_COLOR;
|
|
debug::drawCapsule(core.length, margin, error, out);
|
|
|
|
if (drawCore)
|
|
{
|
|
out << GEOMETRY_CORE_COLOR;
|
|
debug::drawCapsule(core.length, 0, error, out);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case PxConvexCore::eBOX:
|
|
{
|
|
const PxConvexCore::Box& core = convex.getCore<PxConvexCore::Box>();
|
|
const PxReal error = 0.001f;
|
|
|
|
out << pose;
|
|
out << GEOMETRY_COLOR;
|
|
debug::drawBox(core.extents, margin, error, out);
|
|
|
|
if (drawCore)
|
|
{
|
|
out << GEOMETRY_CORE_COLOR;
|
|
debug::drawBox(core.extents, 0, error, out);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case PxConvexCore::eELLIPSOID:
|
|
{
|
|
const PxConvexCore::Ellipsoid& core = convex.getCore<PxConvexCore::Ellipsoid>();
|
|
const PxReal error = 0.001f;
|
|
|
|
out << pose;
|
|
out << GEOMETRY_COLOR;
|
|
debug::drawEllipsoid(core.radii, margin, error, out);
|
|
|
|
if (drawCore)
|
|
{
|
|
out << GEOMETRY_CORE_COLOR;
|
|
debug::drawEllipsoid(core.radii, 0, error, out);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case PxConvexCore::eCYLINDER:
|
|
{
|
|
const PxConvexCore::Cylinder& core = convex.getCore<PxConvexCore::Cylinder>();
|
|
const PxReal height = core.height;
|
|
const PxReal radius = core.radius;
|
|
const PxReal error = 0.001f;
|
|
|
|
out << pose;
|
|
out << GEOMETRY_COLOR;
|
|
debug::drawCylinder(height, radius, margin, error, out);
|
|
|
|
if (drawCore)
|
|
{
|
|
out << GEOMETRY_CORE_COLOR;
|
|
debug::drawCylinder(height, radius, 0, error, out);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case PxConvexCore::eCONE:
|
|
{
|
|
const PxConvexCore::Cone& core = convex.getCore<PxConvexCore::Cone>();
|
|
const PxReal height = core.height;
|
|
const PxReal radius = core.radius;
|
|
const PxReal error = 0.001f;
|
|
|
|
out << pose;
|
|
out << GEOMETRY_COLOR;
|
|
debug::drawCone(height, radius, margin, error, out);
|
|
|
|
if (drawCore)
|
|
{
|
|
out << GEOMETRY_CORE_COLOR;
|
|
debug::drawCone(height, radius, 0, error, out);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
out << pose;
|
|
out << GEOMETRY_COLOR;
|
|
out << PxDebugBox(Gu::computeBounds(convex, PxTransform(PxIdentity)));
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
PX_PHYSX_COMMON_API bool Gu::makeConvexShape(const PxGeometry& geom, const PxTransform& pose, ConvexShape& convex)
|
|
{
|
|
convex.coreType = Gu::ConvexCore::Type::Enum(-1);
|
|
convex.pose = pose;
|
|
|
|
switch (geom.getType())
|
|
{
|
|
case PxGeometryType::eCONVEXCORE:
|
|
{
|
|
const PxConvexCoreGeometry& g = static_cast<const PxConvexCoreGeometry&>(geom);
|
|
convex.coreType = Gu::ConvexCore::Type::Enum(g.getCoreType());
|
|
PxMemCopy(convex.coreData, g.getCoreData(), PxConvexCoreGeometry::MAX_CORE_SIZE);
|
|
convex.margin = g.getMargin();
|
|
return true;
|
|
}
|
|
case PxGeometryType::eSPHERE:
|
|
{
|
|
const PxSphereGeometry& g = static_cast<const PxSphereGeometry&>(geom);
|
|
convex.coreType = Gu::ConvexCore::Type::ePOINT;
|
|
convex.margin = g.radius;
|
|
return true;
|
|
}
|
|
case PxGeometryType::eCAPSULE:
|
|
{
|
|
const PxCapsuleGeometry& g = static_cast<const PxCapsuleGeometry&>(geom);
|
|
convex.coreType = Gu::ConvexCore::Type::eSEGMENT;
|
|
Gu::ConvexCore::SegmentCore& core = *reinterpret_cast<Gu::ConvexCore::SegmentCore*>(convex.coreData);
|
|
core.length = g.halfHeight * 2.0f;
|
|
convex.margin = g.radius;
|
|
return true;
|
|
}
|
|
case PxGeometryType::eBOX:
|
|
{
|
|
const PxBoxGeometry& g = static_cast<const PxBoxGeometry&>(geom);
|
|
convex.coreType = Gu::ConvexCore::Type::eBOX;
|
|
Gu::ConvexCore::BoxCore& core = *reinterpret_cast<Gu::ConvexCore::BoxCore*>(convex.coreData);
|
|
core.extents = g.halfExtents * 2.0f;
|
|
convex.margin = 0;
|
|
return true;
|
|
}
|
|
case PxGeometryType::eCONVEXMESH:
|
|
{
|
|
const PxConvexMeshGeometry& g = static_cast<const PxConvexMeshGeometry&>(geom);
|
|
convex.coreType = Gu::ConvexCore::Type::ePOINTS;
|
|
Gu::ConvexCore::PointsCore& core = *reinterpret_cast<Gu::ConvexCore::PointsCore*>(convex.coreData);
|
|
core.points = g.convexMesh->getVertices();
|
|
core.numPoints = PxU8(g.convexMesh->getNbVertices());
|
|
core.stride = sizeof(PxVec3);
|
|
core.S = g.scale.scale;
|
|
core.R = g.scale.rotation;
|
|
convex.margin = 0;
|
|
return true;
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|