106 lines
4.7 KiB
C++
106 lines
4.7 KiB
C++
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
|
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#include "UnitTestFramework.h"
|
|
#include "PhysicsTestContext.h"
|
|
#include <Jolt/Physics/Collision/Shape/ScaledShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/BoxShape.h>
|
|
#include <Jolt/Physics/Collision/Shape/RotatedTranslatedShape.h>
|
|
#include <Jolt/Physics/Collision/TransformedShape.h>
|
|
#include <Jolt/Physics/Collision/CollisionCollectorImpl.h>
|
|
#include <Jolt/Physics/Collision/RayCast.h>
|
|
#include <Jolt/Physics/Collision/CastResult.h>
|
|
#include <Jolt/Physics/Collision/PhysicsMaterialSimple.h>
|
|
|
|
TEST_SUITE("TransformedShapeTests")
|
|
{
|
|
TEST_CASE("TestTransformedShape")
|
|
{
|
|
const Vec3 half_extents(0.5f, 1.0f, 1.5f);
|
|
const Vec3 scale(-2, 3, 4);
|
|
const Vec3 rtshape_translation(1, 3, 5);
|
|
const Quat rtshape_rotation = Quat::sRotation(Vec3(1, 2, 3).Normalized(), 0.25f * JPH_PI);
|
|
const RVec3 translation(13, 9, 7);
|
|
const Quat rotation = Quat::sRotation(Vec3::sAxisY(), 0.5f * JPH_PI); // A rotation of 90 degrees in order to not shear the shape
|
|
|
|
PhysicsMaterialSimple *material = new PhysicsMaterialSimple("Test Material", Color::sRed);
|
|
|
|
// Create a scaled, rotated and translated box
|
|
BoxShapeSettings box_settings(half_extents, 0.0f, material);
|
|
box_settings.SetEmbedded();
|
|
ScaledShapeSettings scale_settings(&box_settings, scale);
|
|
scale_settings.SetEmbedded();
|
|
RotatedTranslatedShapeSettings rtshape_settings(rtshape_translation, rtshape_rotation, &scale_settings);
|
|
rtshape_settings.SetEmbedded();
|
|
|
|
// Create a body with this shape
|
|
PhysicsTestContext c;
|
|
Body &body = c.CreateBody(&rtshape_settings, translation, rotation, EMotionType::Static, EMotionQuality::Discrete, 0, EActivation::DontActivate);
|
|
|
|
// Collect the leaf shape transform
|
|
AllHitCollisionCollector<TransformedShapeCollector> collector;
|
|
c.GetSystem()->GetNarrowPhaseQuery().CollectTransformedShapes(AABox::sBiggest(), collector);
|
|
|
|
// Check that there is exactly 1 shape
|
|
CHECK(collector.mHits.size() == 1);
|
|
TransformedShape &ts = collector.mHits.front();
|
|
|
|
// Check that we got the leaf shape: box
|
|
CHECK(ts.mShape == box_settings.Create().Get());
|
|
|
|
// Check that its transform matches the transform that we provided
|
|
RMat44 calc_transform = RMat44::sRotationTranslation(rotation, translation) * Mat44::sRotationTranslation(rtshape_rotation, rtshape_translation) * RMat44::sScale(scale);
|
|
CHECK_APPROX_EQUAL(calc_transform, ts.GetWorldTransform());
|
|
|
|
// Check that all corner points are in the bounding box
|
|
AABox aabox = ts.GetWorldSpaceBounds();
|
|
Vec3 corners[] = {
|
|
Vec3(-0.99f, -0.99f, -0.99f) * half_extents,
|
|
Vec3( 0.99f, -0.99f, -0.99f) * half_extents,
|
|
Vec3(-0.99f, 0.99f, -0.99f) * half_extents,
|
|
Vec3( 0.99f, 0.99f, -0.99f) * half_extents,
|
|
Vec3(-0.99f, -0.99f, 0.99f) * half_extents,
|
|
Vec3( 0.99f, -0.99f, 0.99f) * half_extents,
|
|
Vec3(-0.99f, 0.99f, 0.99f) * half_extents,
|
|
Vec3( 0.99f, 0.99f, 0.99f) * half_extents
|
|
};
|
|
for (Vec3 corner : corners)
|
|
{
|
|
CHECK(aabox.Contains(calc_transform * corner));
|
|
CHECK(!aabox.Contains(calc_transform * (2 * corner))); // Check that points twice as far away are not in the box
|
|
}
|
|
|
|
// Now pick a point on the box near the edge in local space, determine a raycast that hits it
|
|
const Vec3 point_on_box(half_extents.GetX() - 0.01f, half_extents.GetY() - 0.01f, half_extents.GetZ());
|
|
const Vec3 normal_on_box(0, 0, 1);
|
|
const Vec3 ray_direction_local(1, 1, -1);
|
|
|
|
// Transform to world space and do the raycast
|
|
Vec3 ray_start_local = point_on_box - ray_direction_local;
|
|
Vec3 ray_end_local = point_on_box + ray_direction_local;
|
|
RVec3 ray_start_world = calc_transform * ray_start_local;
|
|
RVec3 ray_end_world = calc_transform * ray_end_local;
|
|
Vec3 ray_direction_world = Vec3(ray_end_world - ray_start_world);
|
|
RRayCast ray_in_world { ray_start_world, ray_direction_world };
|
|
RayCastResult hit;
|
|
ts.CastRay(ray_in_world, hit);
|
|
|
|
// Check the hit result
|
|
CHECK_APPROX_EQUAL(hit.mFraction, 0.5f);
|
|
CHECK(hit.mBodyID == body.GetID());
|
|
CHECK(ts.GetMaterial(hit.mSubShapeID2) == material);
|
|
Vec3 world_space_normal = ts.GetWorldSpaceSurfaceNormal(hit.mSubShapeID2, ray_in_world.GetPointOnRay(hit.mFraction));
|
|
Vec3 expected_normal = (calc_transform.GetDirectionPreservingMatrix() * normal_on_box).Normalized();
|
|
CHECK_APPROX_EQUAL(world_space_normal, expected_normal);
|
|
|
|
// Reset the transform to identity and check that it worked
|
|
ts.SetWorldTransform(RMat44::sIdentity());
|
|
CHECK_APPROX_EQUAL(ts.GetWorldTransform(), RMat44::sIdentity());
|
|
|
|
// Set the calculated world transform again to see if getting/setting a transform is symmetric
|
|
ts.SetWorldTransform(calc_transform);
|
|
CHECK_APPROX_EQUAL(calc_transform, ts.GetWorldTransform());
|
|
}
|
|
}
|