116 lines
5.2 KiB
C++
116 lines
5.2 KiB
C++
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
|
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "UnitTestFramework.h"
|
|
#include <Jolt/Physics/Collision/BroadPhase/BroadPhaseQuadTree.h>
|
|
#include <Jolt/Physics/Collision/Shape/BoxShape.h>
|
|
#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
|
|
#include <Jolt/Physics/Collision/RayCast.h>
|
|
#include <Jolt/Physics/Collision/CastResult.h>
|
|
#include <Jolt/Physics/Body/BodyManager.h>
|
|
#include <Jolt/Physics/Body/BodyCreationSettings.h>
|
|
#include "Layers.h"
|
|
|
|
TEST_SUITE("BroadPhaseTests")
|
|
{
|
|
TEST_CASE("TestBroadPhaseOptimize")
|
|
{
|
|
BPLayerInterfaceImpl broad_phase_layer_interface;
|
|
|
|
// Create body manager
|
|
BodyManager body_manager;
|
|
body_manager.Init(1, 0, broad_phase_layer_interface);
|
|
|
|
// Create quad tree
|
|
BroadPhaseQuadTree broadphase;
|
|
broadphase.Init(&body_manager, broad_phase_layer_interface);
|
|
|
|
// Create a box
|
|
BodyCreationSettings settings(new BoxShape(Vec3::sOne()), RVec3::sZero(), Quat::sIdentity(), EMotionType::Static, Layers::NON_MOVING);
|
|
Body &body = *body_manager.AllocateBody(settings);
|
|
body_manager.AddBody(&body);
|
|
|
|
// Add it to the broadphase
|
|
BodyID id = body.GetID();
|
|
BroadPhase::AddState add_state = broadphase.AddBodiesPrepare(&id, 1);
|
|
broadphase.AddBodiesFinalize(&id, 1, add_state);
|
|
|
|
// Test that we hit the box at its current location and not where we're going to move it to
|
|
AllHitCollisionCollector<RayCastBodyCollector> collector;
|
|
broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.size() == 1);
|
|
CHECK(collector.mHits[0].mBodyID == id);
|
|
CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
|
|
collector.Reset();
|
|
broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.empty());
|
|
broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.empty());
|
|
|
|
// Move the body
|
|
body.SetPositionAndRotationInternal(RVec3(2, 0, 0), Quat::sIdentity());
|
|
broadphase.NotifyBodiesAABBChanged(&id, 1, true);
|
|
|
|
// Test that we hit the box at its previous and current location
|
|
broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.size() == 1);
|
|
CHECK(collector.mHits[0].mBodyID == id);
|
|
CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
|
|
collector.Reset();
|
|
broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.size() == 1);
|
|
CHECK(collector.mHits[0].mBodyID == id);
|
|
CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
|
|
collector.Reset();
|
|
broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.empty());
|
|
|
|
// Optimize the broadphase
|
|
broadphase.Optimize();
|
|
|
|
// Test that we hit the box only at the new location
|
|
broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.empty());
|
|
broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.size() == 1);
|
|
CHECK(collector.mHits[0].mBodyID == id);
|
|
CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
|
|
collector.Reset();
|
|
broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.empty());
|
|
|
|
// Move the body again (so that for the next optimize we'll have to discard a tree)
|
|
body.SetPositionAndRotationInternal(RVec3(4, 0, 0), Quat::sIdentity());
|
|
broadphase.NotifyBodiesAABBChanged(&id, 1, true);
|
|
|
|
// Test that we hit the box at its previous and current location
|
|
broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.empty());
|
|
broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.size() == 1);
|
|
CHECK(collector.mHits[0].mBodyID == id);
|
|
CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
|
|
collector.Reset();
|
|
broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.size() == 1);
|
|
CHECK(collector.mHits[0].mBodyID == id);
|
|
CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
|
|
collector.Reset();
|
|
|
|
// Optimize the broadphase (this will internally have to discard a tree)
|
|
broadphase.Optimize();
|
|
|
|
// Test that we hit the box only at the new location
|
|
broadphase.CastRay({ Vec3(0, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.empty());
|
|
broadphase.CastRay({ Vec3(2, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.empty());
|
|
broadphase.CastRay({ Vec3(4, 2, 0), Vec3(0, -2, 0) }, collector, BroadPhaseLayerFilter(), ObjectLayerFilter());
|
|
CHECK(collector.mHits.size() == 1);
|
|
CHECK(collector.mHits[0].mBodyID == id);
|
|
CHECK_APPROX_EQUAL(collector.mHits[0].mFraction, 0.5f);
|
|
collector.Reset();
|
|
}
|
|
}
|