73 lines
3.4 KiB
C++
73 lines
3.4 KiB
C++
|
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
||
|
|
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
|
||
|
|
// SPDX-License-Identifier: MIT
|
||
|
|
|
||
|
|
#include <TestFramework.h>
|
||
|
|
|
||
|
|
#include <Tests/ConvexCollision/CapsuleVsBoxTest.h>
|
||
|
|
#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
|
||
|
|
#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
|
||
|
|
#include <Jolt/Physics/Collision/Shape/BoxShape.h>
|
||
|
|
#include <Jolt/Physics/Collision/CollideShape.h>
|
||
|
|
#include <Jolt/Physics/Collision/CollisionDispatch.h>
|
||
|
|
#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
|
||
|
|
#include <Utils/DebugRendererSP.h>
|
||
|
|
|
||
|
|
JPH_IMPLEMENT_RTTI_VIRTUAL(CapsuleVsBoxTest)
|
||
|
|
{
|
||
|
|
JPH_ADD_BASE_CLASS(CapsuleVsBoxTest, Test)
|
||
|
|
}
|
||
|
|
|
||
|
|
void CapsuleVsBoxTest::PrePhysicsUpdate(const PreUpdateParams &inParams)
|
||
|
|
{
|
||
|
|
// Create box
|
||
|
|
Vec3 box_min(-1.0f, -2.0f, 0.5f);
|
||
|
|
Vec3 box_max(2.0f, -0.5f, 3.0f);
|
||
|
|
Ref<RotatedTranslatedShapeSettings> box_settings = new RotatedTranslatedShapeSettings(0.5f * (box_min + box_max), Quat::sIdentity(), new BoxShapeSettings(0.5f * (box_max - box_min)));
|
||
|
|
Ref<Shape> box_shape = box_settings->Create().Get();
|
||
|
|
Mat44 box_transform(Vec4(0.516170502f, -0.803887904f, -0.295520246f, 0.0f), Vec4(0.815010250f, 0.354940295f, 0.458012700f, 0.0f), Vec4(-0.263298869f, -0.477264702f, 0.838386655f, 0.0f), Vec4(-10.2214508f, -18.6808319f, 40.7468987f, 1.0f));
|
||
|
|
|
||
|
|
// Create capsule
|
||
|
|
float capsule_half_height = 75.0f;
|
||
|
|
float capsule_radius = 1.5f;
|
||
|
|
Quat capsule_compound_rotation(0.499999970f, -0.499999970f, -0.499999970f, 0.499999970f);
|
||
|
|
Ref<RotatedTranslatedShapeSettings> capsule_settings = new RotatedTranslatedShapeSettings(Vec3(0, 0, 75), capsule_compound_rotation, new CapsuleShapeSettings(capsule_half_height, capsule_radius));
|
||
|
|
Ref<Shape> capsule_shape = capsule_settings->Create().Get();
|
||
|
|
Mat44 capsule_transform = Mat44::sTranslation(Vec3(-9.68538570f, -18.0328083f, 41.3212280f));
|
||
|
|
|
||
|
|
// Collision settings
|
||
|
|
CollideShapeSettings settings;
|
||
|
|
settings.mActiveEdgeMode = EActiveEdgeMode::CollideWithAll;
|
||
|
|
settings.mBackFaceMode = EBackFaceMode::CollideWithBackFaces;
|
||
|
|
settings.mCollectFacesMode = ECollectFacesMode::NoFaces;
|
||
|
|
|
||
|
|
// Collide the two shapes
|
||
|
|
AllHitCollisionCollector<CollideShapeCollector> collector;
|
||
|
|
CollisionDispatch::sCollideShapeVsShape(capsule_shape, box_shape, Vec3::sOne(), Vec3::sOne(), capsule_transform, box_transform, SubShapeIDCreator(), SubShapeIDCreator(), settings, collector);
|
||
|
|
|
||
|
|
#ifdef JPH_DEBUG_RENDERER
|
||
|
|
// Draw the shapes
|
||
|
|
box_shape->Draw(mDebugRenderer, RMat44(box_transform), Vec3::sOne(), Color::sWhite, false, false);
|
||
|
|
capsule_shape->Draw(mDebugRenderer, RMat44(capsule_transform), Vec3::sOne(), Color::sWhite, false, false);
|
||
|
|
#endif // JPH_DEBUG_RENDERER
|
||
|
|
|
||
|
|
// Draw contact points
|
||
|
|
const CollideShapeResult &hit = collector.mHits[0];
|
||
|
|
DrawMarkerSP(mDebugRenderer, hit.mContactPointOn1, Color::sRed, 1.0f);
|
||
|
|
DrawMarkerSP(mDebugRenderer, hit.mContactPointOn2, Color::sGreen, 1.0f);
|
||
|
|
|
||
|
|
// Draw penetration axis with length of the penetration
|
||
|
|
Vec3 pen_axis = hit.mPenetrationAxis;
|
||
|
|
float pen_axis_len = pen_axis.Length();
|
||
|
|
if (pen_axis_len > 0.0f)
|
||
|
|
{
|
||
|
|
pen_axis *= hit.mPenetrationDepth / pen_axis_len;
|
||
|
|
DrawArrowSP(mDebugRenderer, hit.mContactPointOn2, hit.mContactPointOn2 + pen_axis, Color::sYellow, 0.01f);
|
||
|
|
|
||
|
|
#ifdef JPH_DEBUG_RENDERER
|
||
|
|
Mat44 resolved_box = box_transform.PostTranslated(pen_axis);
|
||
|
|
box_shape->Draw(mDebugRenderer, RMat44(resolved_box), Vec3::sOne(), Color::sGreen, false, false);
|
||
|
|
#endif // JPH_DEBUG_RENDERER
|
||
|
|
}
|
||
|
|
}
|