Jorrit Rouwe
d0025bd4a3
Fixed an issue where collide/cast shape against a triangle would return a hit result with mShape2Face in incorrect winding order. This caused an incorrect normal in the enhanced internal edge removal algorithm. This in turn resulted in objects not settling properly on dense triangle grids.
111 lines
5.0 KiB
C++
111 lines
5.0 KiB
C++
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
|
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include <Jolt/Jolt.h>
|
|
|
|
#include <Jolt/Physics/Collision/CastConvexVsTriangles.h>
|
|
#include <Jolt/Physics/Collision/TransformedShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/ScaleHelpers.h>
|
|
#include <Jolt/Physics/Collision/ActiveEdges.h>
|
|
#include <Jolt/Physics/Collision/NarrowPhaseStats.h>
|
|
#include <Jolt/Geometry/EPAPenetrationDepth.h>
|
|
|
|
JPH_NAMESPACE_BEGIN
|
|
|
|
CastConvexVsTriangles::CastConvexVsTriangles(const ShapeCast &inShapeCast, const ShapeCastSettings &inShapeCastSettings, Vec3Arg inScale, Mat44Arg inCenterOfMassTransform2, const SubShapeIDCreator &inSubShapeIDCreator1, CastShapeCollector &ioCollector) :
|
|
mShapeCast(inShapeCast),
|
|
mShapeCastSettings(inShapeCastSettings),
|
|
mCenterOfMassTransform2(inCenterOfMassTransform2),
|
|
mScale(inScale),
|
|
mSubShapeIDCreator1(inSubShapeIDCreator1),
|
|
mCollector(ioCollector)
|
|
{
|
|
JPH_ASSERT(inShapeCast.mShape->GetType() == EShapeType::Convex);
|
|
|
|
// Determine if shape is inside out or not
|
|
mScaleSign = ScaleHelpers::IsInsideOut(inScale)? -1.0f : 1.0f;
|
|
}
|
|
|
|
void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 inActiveEdges, const SubShapeID &inSubShapeID2)
|
|
{
|
|
// Scale triangle
|
|
Vec3 v0 = mScale * inV0;
|
|
Vec3 v1 = mScale * inV1;
|
|
Vec3 v2 = mScale * inV2;
|
|
|
|
// Calculate triangle normal
|
|
Vec3 triangle_normal = mScaleSign * (v1 - v0).Cross(v2 - v0);
|
|
|
|
// Backface check
|
|
bool back_facing = triangle_normal.Dot(mShapeCast.mDirection) > 0.0f;
|
|
if (mShapeCastSettings.mBackFaceModeTriangles == EBackFaceMode::IgnoreBackFaces && back_facing)
|
|
return;
|
|
|
|
// Create triangle support function
|
|
TriangleConvexSupport triangle { v0, v1, v2 };
|
|
|
|
// Check if we already created the cast shape support function
|
|
if (mSupport == nullptr)
|
|
{
|
|
// Determine if we want to use the actual shape or a shrunken shape with convex radius
|
|
ConvexShape::ESupportMode support_mode = mShapeCastSettings.mUseShrunkenShapeAndConvexRadius? ConvexShape::ESupportMode::ExcludeConvexRadius : ConvexShape::ESupportMode::Default;
|
|
|
|
// Create support function
|
|
mSupport = static_cast<const ConvexShape *>(mShapeCast.mShape)->GetSupportFunction(support_mode, mSupportBuffer, mShapeCast.mScale);
|
|
}
|
|
|
|
EPAPenetrationDepth epa;
|
|
float fraction = mCollector.GetEarlyOutFraction();
|
|
Vec3 contact_point_a, contact_point_b, contact_normal;
|
|
if (epa.CastShape(mShapeCast.mCenterOfMassStart, mShapeCast.mDirection, mShapeCastSettings.mCollisionTolerance, mShapeCastSettings.mPenetrationTolerance, *mSupport, triangle, mSupport->GetConvexRadius(), 0.0f, mShapeCastSettings.mReturnDeepestPoint, fraction, contact_point_a, contact_point_b, contact_normal))
|
|
{
|
|
// Check if we have enabled active edge detection
|
|
if (mShapeCastSettings.mActiveEdgeMode == EActiveEdgeMode::CollideOnlyWithActive && inActiveEdges != 0b111)
|
|
{
|
|
// Convert the active edge velocity hint to local space
|
|
Vec3 active_edge_movement_direction = mCenterOfMassTransform2.Multiply3x3Transposed(mShapeCastSettings.mActiveEdgeMovementDirection);
|
|
|
|
// Update the contact normal to account for active edges
|
|
// Note that we flip the triangle normal as the penetration axis is pointing towards the triangle instead of away
|
|
contact_normal = ActiveEdges::FixNormal(v0, v1, v2, back_facing? triangle_normal : -triangle_normal, inActiveEdges, contact_point_b, contact_normal, active_edge_movement_direction);
|
|
}
|
|
|
|
// Convert to world space
|
|
contact_point_a = mCenterOfMassTransform2 * contact_point_a;
|
|
contact_point_b = mCenterOfMassTransform2 * contact_point_b;
|
|
Vec3 contact_normal_world = mCenterOfMassTransform2.Multiply3x3(contact_normal);
|
|
|
|
// Its a hit, store the sub shape id's
|
|
ShapeCastResult result(fraction, contact_point_a, contact_point_b, contact_normal_world, back_facing, mSubShapeIDCreator1.GetID(), inSubShapeID2, TransformedShape::sGetBodyID(mCollector.GetContext()));
|
|
|
|
// Early out if this hit is deeper than the collector's early out value
|
|
if (fraction == 0.0f && -result.mPenetrationDepth >= mCollector.GetEarlyOutFraction())
|
|
return;
|
|
|
|
// Gather faces
|
|
if (mShapeCastSettings.mCollectFacesMode == ECollectFacesMode::CollectFaces)
|
|
{
|
|
// Get supporting face of shape 1
|
|
Mat44 transform_1_to_2 = mShapeCast.mCenterOfMassStart;
|
|
transform_1_to_2.SetTranslation(transform_1_to_2.GetTranslation() + fraction * mShapeCast.mDirection);
|
|
static_cast<const ConvexShape *>(mShapeCast.mShape)->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), mShapeCast.mScale, mCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face);
|
|
|
|
// Get face of the triangle
|
|
result.mShape2Face.resize(3);
|
|
result.mShape2Face[0] = mCenterOfMassTransform2 * v0;
|
|
result.mShape2Face[1] = mCenterOfMassTransform2 * v1;
|
|
result.mShape2Face[2] = mCenterOfMassTransform2 * v2;
|
|
|
|
// When inside out, we need to swap the triangle winding
|
|
if (mScaleSign < 0.0f)
|
|
std::swap(result.mShape2Face[1], result.mShape2Face[2]);
|
|
}
|
|
|
|
JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;)
|
|
mCollector.AddHit(result);
|
|
}
|
|
}
|
|
|
|
JPH_NAMESPACE_END
|