mirror of
https://github.com/godotengine/godot.git
synced 2024-12-21 10:25:24 +08:00
233 lines
11 KiB
C++
233 lines
11 KiB
C++
/**************************************************************************/
|
|
/* jolt_custom_ray_shape.cpp */
|
|
/**************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* https://godotengine.org */
|
|
/**************************************************************************/
|
|
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
|
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
|
/* */
|
|
/* Permission is hereby granted, free of charge, to any person obtaining */
|
|
/* a copy of this software and associated documentation files (the */
|
|
/* "Software"), to deal in the Software without restriction, including */
|
|
/* without limitation the rights to use, copy, modify, merge, publish, */
|
|
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
|
/* permit persons to whom the Software is furnished to do so, subject to */
|
|
/* the following conditions: */
|
|
/* */
|
|
/* The above copyright notice and this permission notice shall be */
|
|
/* included in all copies or substantial portions of the Software. */
|
|
/* */
|
|
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
|
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
|
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
|
|
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
|
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
|
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
|
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
|
/**************************************************************************/
|
|
|
|
#include "jolt_custom_ray_shape.h"
|
|
|
|
#include "../spaces/jolt_query_collectors.h"
|
|
|
|
#include "Jolt/Physics/Collision/CastResult.h"
|
|
#include "Jolt/Physics/Collision/RayCast.h"
|
|
#include "Jolt/Physics/Collision/TransformedShape.h"
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
#include "Jolt/Renderer/DebugRenderer.h"
|
|
#endif
|
|
|
|
namespace {
|
|
|
|
class JoltCustomRayShapeSupport final : public JPH::ConvexShape::Support {
|
|
public:
|
|
explicit JoltCustomRayShapeSupport(float p_length) :
|
|
length(p_length) {}
|
|
|
|
virtual JPH::Vec3 GetSupport(JPH::Vec3Arg p_direction) const override {
|
|
if (p_direction.GetZ() > 0.0f) {
|
|
return JPH::Vec3(0.0f, 0.0f, length);
|
|
} else {
|
|
return JPH::Vec3::sZero();
|
|
}
|
|
}
|
|
|
|
virtual float GetConvexRadius() const override { return 0.0f; }
|
|
|
|
private:
|
|
float length = 0.0f;
|
|
};
|
|
|
|
static_assert(sizeof(JoltCustomRayShapeSupport) <= sizeof(JPH::ConvexShape::SupportBuffer), "Size of SeparationRayShape3D support is larger than size of support buffer.");
|
|
|
|
JPH::Shape *construct_ray() {
|
|
return new JoltCustomRayShape();
|
|
}
|
|
|
|
void collide_ray_vs_shape(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
|
|
ERR_FAIL_COND(p_shape1->GetSubType() != JoltCustomShapeSubType::RAY);
|
|
|
|
const JoltCustomRayShape *shape1 = static_cast<const JoltCustomRayShape *>(p_shape1);
|
|
|
|
const float margin = p_collide_shape_settings.mMaxSeparationDistance;
|
|
const float ray_length = shape1->length;
|
|
const float ray_length_padded = ray_length + margin;
|
|
|
|
const JPH::Mat44 transform1 = p_center_of_mass_transform1 * JPH::Mat44::sScale(p_scale1);
|
|
const JPH::Mat44 transform2 = p_center_of_mass_transform2 * JPH::Mat44::sScale(p_scale2);
|
|
const JPH::Mat44 transform_inv2 = transform2.Inversed();
|
|
|
|
const JPH::Vec3 ray_start = transform1.GetTranslation();
|
|
const JPH::Vec3 ray_direction = transform1.GetAxisZ();
|
|
const JPH::Vec3 ray_vector = ray_direction * ray_length;
|
|
const JPH::Vec3 ray_vector_padded = ray_direction * ray_length_padded;
|
|
|
|
const JPH::Vec3 ray_start2 = transform_inv2 * ray_start;
|
|
const JPH::Vec3 ray_direction2 = transform_inv2.Multiply3x3(ray_direction);
|
|
const JPH::Vec3 ray_vector_padded2 = transform_inv2.Multiply3x3(ray_vector_padded);
|
|
|
|
const JPH::RayCast ray_cast(ray_start2, ray_vector_padded2);
|
|
|
|
JPH::RayCastSettings ray_cast_settings;
|
|
ray_cast_settings.mTreatConvexAsSolid = false;
|
|
ray_cast_settings.mBackFaceModeTriangles = p_collide_shape_settings.mBackFaceMode;
|
|
|
|
JoltQueryCollectorClosest<JPH::CastRayCollector> ray_collector;
|
|
|
|
p_shape2->CastRay(ray_cast, ray_cast_settings, p_sub_shape_id_creator2, ray_collector);
|
|
|
|
if (!ray_collector.had_hit()) {
|
|
return;
|
|
}
|
|
|
|
const JPH::RayCastResult &hit = ray_collector.get_hit();
|
|
|
|
const float hit_distance = ray_length_padded * hit.mFraction;
|
|
const float hit_depth = ray_length - hit_distance;
|
|
|
|
if (-hit_depth >= p_collector.GetEarlyOutFraction()) {
|
|
return;
|
|
}
|
|
|
|
// Since `hit.mSubShapeID2` could represent a path not only from `p_shape2` but also any
|
|
// compound shape that it's contained within, we need to split this path into something that
|
|
// `p_shape2` can actually understand.
|
|
JPH::SubShapeID local_sub_shape_id2;
|
|
hit.mSubShapeID2.PopID(p_sub_shape_id_creator2.GetNumBitsWritten(), local_sub_shape_id2);
|
|
|
|
const JPH::Vec3 hit_point2 = ray_cast.GetPointOnRay(hit.mFraction);
|
|
|
|
const JPH::Vec3 hit_point_on_1 = ray_start + ray_vector;
|
|
const JPH::Vec3 hit_point_on_2 = transform2 * hit_point2;
|
|
|
|
JPH::Vec3 hit_normal2 = JPH::Vec3::sZero();
|
|
|
|
if (shape1->slide_on_slope) {
|
|
hit_normal2 = p_shape2->GetSurfaceNormal(local_sub_shape_id2, hit_point2);
|
|
|
|
// If we got a back-face normal we need to flip it.
|
|
if (hit_normal2.Dot(ray_direction2) > 0) {
|
|
hit_normal2 = -hit_normal2;
|
|
}
|
|
} else {
|
|
hit_normal2 = -ray_direction2;
|
|
}
|
|
|
|
const JPH::Vec3 hit_normal = transform2.Multiply3x3(hit_normal2);
|
|
|
|
JPH::CollideShapeResult result(hit_point_on_1, hit_point_on_2, -hit_normal, hit_depth, p_sub_shape_id_creator1.GetID(), hit.mSubShapeID2, JPH::TransformedShape::sGetBodyID(p_collector.GetContext()));
|
|
|
|
if (p_collide_shape_settings.mCollectFacesMode == JPH::ECollectFacesMode::CollectFaces) {
|
|
p_shape2->GetSupportingFace(local_sub_shape_id2, ray_direction2, p_scale2, p_center_of_mass_transform2, result.mShape2Face);
|
|
}
|
|
|
|
p_collector.AddHit(result);
|
|
}
|
|
|
|
void collide_noop(const JPH::Shape *p_shape1, const JPH::Shape *p_shape2, JPH::Vec3Arg p_scale1, JPH::Vec3Arg p_scale2, JPH::Mat44Arg p_center_of_mass_transform1, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, const JPH::CollideShapeSettings &p_collide_shape_settings, JPH::CollideShapeCollector &p_collector, const JPH::ShapeFilter &p_shape_filter) {
|
|
}
|
|
|
|
void cast_noop(const JPH::ShapeCast &p_shape_cast, const JPH::ShapeCastSettings &p_shape_cast_settings, const JPH::Shape *p_shape, JPH::Vec3Arg p_scale, const JPH::ShapeFilter &p_shape_filter, JPH::Mat44Arg p_center_of_mass_transform2, const JPH::SubShapeIDCreator &p_sub_shape_id_creator1, const JPH::SubShapeIDCreator &p_sub_shape_id_creator2, JPH::CastShapeCollector &p_collector) {
|
|
}
|
|
|
|
} // namespace
|
|
|
|
JPH::ShapeSettings::ShapeResult JoltCustomRayShapeSettings::Create() const {
|
|
if (mCachedResult.IsEmpty()) {
|
|
new JoltCustomRayShape(*this, mCachedResult);
|
|
}
|
|
|
|
return mCachedResult;
|
|
}
|
|
|
|
void JoltCustomRayShape::register_type() {
|
|
JPH::ShapeFunctions &shape_functions = JPH::ShapeFunctions::sGet(JoltCustomShapeSubType::RAY);
|
|
|
|
shape_functions.mConstruct = construct_ray;
|
|
shape_functions.mColor = JPH::Color::sDarkRed;
|
|
|
|
static constexpr JPH::EShapeSubType concrete_sub_types[] = {
|
|
JPH::EShapeSubType::Sphere,
|
|
JPH::EShapeSubType::Box,
|
|
JPH::EShapeSubType::Triangle,
|
|
JPH::EShapeSubType::Capsule,
|
|
JPH::EShapeSubType::TaperedCapsule,
|
|
JPH::EShapeSubType::Cylinder,
|
|
JPH::EShapeSubType::ConvexHull,
|
|
JPH::EShapeSubType::Mesh,
|
|
JPH::EShapeSubType::HeightField,
|
|
JPH::EShapeSubType::Plane,
|
|
JPH::EShapeSubType::TaperedCylinder
|
|
};
|
|
|
|
for (const JPH::EShapeSubType concrete_sub_type : concrete_sub_types) {
|
|
JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::RAY, concrete_sub_type, collide_ray_vs_shape);
|
|
JPH::CollisionDispatch::sRegisterCollideShape(concrete_sub_type, JoltCustomShapeSubType::RAY, JPH::CollisionDispatch::sReversedCollideShape);
|
|
}
|
|
|
|
JPH::CollisionDispatch::sRegisterCollideShape(JoltCustomShapeSubType::RAY, JoltCustomShapeSubType::RAY, collide_noop);
|
|
|
|
for (const JPH::EShapeSubType sub_type : JPH::sAllSubShapeTypes) {
|
|
JPH::CollisionDispatch::sRegisterCastShape(JoltCustomShapeSubType::RAY, sub_type, cast_noop);
|
|
JPH::CollisionDispatch::sRegisterCastShape(sub_type, JoltCustomShapeSubType::RAY, cast_noop);
|
|
}
|
|
}
|
|
|
|
JPH::AABox JoltCustomRayShape::GetLocalBounds() const {
|
|
const float radius = GetInnerRadius();
|
|
return JPH::AABox(JPH::Vec3(-radius, -radius, 0.0f), JPH::Vec3(radius, radius, length));
|
|
}
|
|
|
|
float JoltCustomRayShape::GetInnerRadius() const {
|
|
// There is no sensible value here, since this shape is infinitely thin, so we pick something
|
|
// that's hopefully small enough to effectively be zero, but big enough to not cause any
|
|
// numerical issues.
|
|
return 0.0001f;
|
|
}
|
|
|
|
JPH::MassProperties JoltCustomRayShape::GetMassProperties() const {
|
|
JPH::MassProperties mass_properties;
|
|
|
|
// Since this shape has no volume we can't really give it a correct set of mass properties, so
|
|
// instead we just give it some arbitrary ones.
|
|
mass_properties.mMass = 1.0f;
|
|
mass_properties.mInertia = JPH::Mat44::sIdentity();
|
|
|
|
return mass_properties;
|
|
}
|
|
|
|
#ifdef JPH_DEBUG_RENDERER
|
|
|
|
void JoltCustomRayShape::Draw(JPH::DebugRenderer *p_renderer, JPH::RMat44Arg p_center_of_mass_transform, JPH::Vec3Arg p_scale, JPH::ColorArg p_color, bool p_use_material_colors, bool p_draw_wireframe) const {
|
|
p_renderer->DrawArrow(p_center_of_mass_transform.GetTranslation(), p_center_of_mass_transform * JPH::Vec3(0, 0, length * p_scale.GetZ()), p_use_material_colors ? GetMaterial()->GetDebugColor() : p_color, 0.1f);
|
|
}
|
|
|
|
#endif
|
|
|
|
const JPH::ConvexShape::Support *JoltCustomRayShape::GetSupportFunction(JPH::ConvexShape::ESupportMode p_mode, JPH::ConvexShape::SupportBuffer &p_buffer, JPH::Vec3Arg p_scale) const {
|
|
return new (&p_buffer) JoltCustomRayShapeSupport(p_scale.GetZ() * length);
|
|
}
|