mirror of
https://github.com/zeldaret/botw
synced 2026-07-10 06:16:55 -04:00
ksys/phys: Add ShapeCast
This commit is contained in:
Léo Lam
@@ -0,0 +1,256 @@
|
||||
#include "KingSystem/Physics/System/physShapeCast.h"
|
||||
#include <Havok/Physics2012/Collide/Agent/Collidable/hkpCdPoint.h>
|
||||
#include <Havok/Physics2012/Collide/Agent/hkpProcessCollisionInput.h>
|
||||
#include <Havok/Physics2012/Collide/Query/CastUtil/hkpLinearCastInput.h>
|
||||
#include <Havok/Physics2012/Collide/Query/Collector/PointCollector/hkpAllCdPointCollector.h>
|
||||
#include "KingSystem/Physics/System/physContactMgr.h"
|
||||
#include "KingSystem/Physics/System/physEntityContactListener.h"
|
||||
#include "KingSystem/Physics/System/physGroupFilter.h"
|
||||
#include "KingSystem/Physics/System/physQueryContactPointInfo.h"
|
||||
#include "KingSystem/Physics/System/physSystem.h"
|
||||
#include "KingSystem/Physics/physConversions.h"
|
||||
#include "KingSystem/Physics/physLayerMaskBuilder.h"
|
||||
|
||||
namespace ksys::phys {
|
||||
|
||||
ShapeCast::ShapeCast(RigidBody* body, QueryContactPointInfo* contact_point_info, Mode mode)
|
||||
: mContactPointInfo(contact_point_info), mMode(mode) {
|
||||
if (body)
|
||||
mBody = body;
|
||||
|
||||
mTo = sead::Vector3f::zero;
|
||||
mFrom = sead::Vector3f::zero;
|
||||
mRotation.set(1, 0, 0, 0);
|
||||
reset();
|
||||
}
|
||||
|
||||
ShapeCast::~ShapeCast() = default;
|
||||
|
||||
void ShapeCast::reset() {
|
||||
_40 = {};
|
||||
_41 = {};
|
||||
_42 = {};
|
||||
_44 = {};
|
||||
|
||||
if (mContactPointInfo)
|
||||
mContactPointInfo->getNumContactPoints() = 0;
|
||||
}
|
||||
|
||||
void ShapeCast::setRotation(sead::Quatf rotation) {
|
||||
mRotation = rotation;
|
||||
}
|
||||
|
||||
void ShapeCast::setRotation(const sead::Matrix33f& rotation_matrix) {
|
||||
auto quat = sead::Quatf::unit;
|
||||
rotation_matrix.toQuat(quat);
|
||||
setRotation(quat);
|
||||
}
|
||||
|
||||
void ShapeCast::setRotation(const sead::Matrix34f& transform_matrix) {
|
||||
auto quat = sead::Quatf::unit;
|
||||
transform_matrix.toQuat(quat);
|
||||
setRotation(quat);
|
||||
}
|
||||
|
||||
void ShapeCast::setStartAndEnd(const sead::Vector3f& start, const sead::Vector3f& end) {
|
||||
mFrom = start;
|
||||
mTo = end;
|
||||
}
|
||||
|
||||
void ShapeCast::setStartAndDisplacement(const sead::Vector3f& start,
|
||||
const sead::Vector3f& displacement) {
|
||||
mFrom = start;
|
||||
mTo = start + displacement;
|
||||
}
|
||||
|
||||
void ShapeCast::setStartAndDisplacementScaled(const sead::Vector3f& start,
|
||||
const sead::Vector3f& displacement,
|
||||
float displacement_scale) {
|
||||
mFrom = start;
|
||||
mTo = start + displacement * displacement_scale;
|
||||
}
|
||||
|
||||
void ShapeCast::setLayerMasks(const LayerMaskBuilder& mask_builder) {
|
||||
mContactPointInfo->setLayerMasks(mask_builder);
|
||||
}
|
||||
|
||||
void ShapeCast::setColors(const sead::Color4f& color1, const sead::Color4f& color2,
|
||||
const sead::Color4f& color3, bool enabled) {
|
||||
// Stubbed in release builds.
|
||||
}
|
||||
|
||||
// NON_MATCHING: the last if block is a complete mess. Should be equivalent, though.
|
||||
bool ShapeCast::doExecuteQuery(hkpCdPointCollector& cast_collector,
|
||||
hkpAllCdPointCollector* start_collector,
|
||||
ShapeCast::WeldClosestPoints weld_closest_points,
|
||||
OnlyLockIfNeeded only_lock_if_needed) {
|
||||
const auto layer_type = mBody->getLayerType();
|
||||
|
||||
System::instance()->lockWorld(layer_type, "shape_cast", 0, only_lock_if_needed);
|
||||
|
||||
hkpLinearCastInput input;
|
||||
loadFromVec3(&input.m_to, mTo);
|
||||
|
||||
// Reset internal state.
|
||||
cast_collector.reset();
|
||||
if (_44 == 1)
|
||||
reset();
|
||||
|
||||
System::instance()->getHavokWorld(layer_type)->m_collisionInput->m_weldClosestPoints =
|
||||
bool(weld_closest_points);
|
||||
|
||||
doCast(input, cast_collector, start_collector);
|
||||
|
||||
System::instance()->unlockWorld(layer_type, "shape_cast", 0, only_lock_if_needed);
|
||||
|
||||
// Register every point that is in start_collector.
|
||||
auto* body = mBody;
|
||||
for (int i = 0, n = start_collector->getNumHits(); i < n; ++i) {
|
||||
if (registerContactPoint(start_collector->getHits()[i], body, ClampDistance::Yes)) {
|
||||
_40 = true;
|
||||
if (mMode == Mode::_0)
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
_41 = _40;
|
||||
|
||||
if (mMode == Mode::_2 || !_40) {
|
||||
int num_hits = 0;
|
||||
FilteredClosestCdPointCollector* filtered_collector = nullptr;
|
||||
hkpAllCdPointCollector* all_point_collector = nullptr;
|
||||
bool ok = false;
|
||||
|
||||
if (mMode == Mode::_1 || mMode == Mode::_0) {
|
||||
filtered_collector = static_cast<FilteredClosestCdPointCollector*>(&cast_collector);
|
||||
num_hits = 1;
|
||||
ok = filtered_collector->hasHit();
|
||||
} else {
|
||||
all_point_collector = static_cast<hkpAllCdPointCollector*>(&cast_collector);
|
||||
num_hits = all_point_collector->getNumHits();
|
||||
ok = num_hits >= 0;
|
||||
}
|
||||
|
||||
if (ok) {
|
||||
for (int i = 0; i < num_hits; ++i) {
|
||||
const auto& point = mMode == Mode::_2 ? all_point_collector->getHits()[i] :
|
||||
filtered_collector->getHit();
|
||||
if (registerContactPoint(point, body, ClampDistance::No))
|
||||
_40 = true;
|
||||
}
|
||||
}
|
||||
|
||||
_42 = _40;
|
||||
}
|
||||
|
||||
_44 = 1;
|
||||
return _40;
|
||||
}
|
||||
|
||||
bool ShapeCast::registerContactPoint(const hkpRootCdPoint& point, RigidBody* body,
|
||||
ClampDistance clamp_distance) {
|
||||
auto* hit_entity = getHkpEntity(*point.m_rootCollidableB);
|
||||
if (!hit_entity)
|
||||
return false;
|
||||
|
||||
if (!mContactPointInfo->testContactPointDistance(point.getContact().getDistance()))
|
||||
return false;
|
||||
|
||||
auto* hit_body = getRigidBody(hit_entity);
|
||||
|
||||
if (System::instance()->getEntityContactListenerField91() && hit_body->isEntity() &&
|
||||
EntityContactListener::isObjectOrGroundOrNPCOrTree(*hit_body)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
RigidBodyCollisionMasks masks_a, masks_b;
|
||||
sead::Vector3f contact_position;
|
||||
storeToVec3(&contact_position, point.getContact().getPosition());
|
||||
|
||||
body->getCollisionMasks(&masks_a, &point.m_shapeKeyA, contact_position);
|
||||
hit_body->getCollisionMasks(&masks_b, &point.m_shapeKeyB, contact_position);
|
||||
|
||||
auto* filter = System::instance()->getGroupFilter(hit_body->getLayerType());
|
||||
auto layer_b = filter->getCollisionFilterInfoLayer(masks_b.collision_filter_info);
|
||||
|
||||
if (!mContactPointInfo->isLayerSubscribed(layer_b))
|
||||
return false;
|
||||
|
||||
ContactPoint contact_point;
|
||||
contact_point.flags.makeAllZero();
|
||||
contact_point.body_a = body;
|
||||
contact_point.body_b = hit_body;
|
||||
contact_point.material_mask_a = MaterialMaskData(masks_a.material_mask);
|
||||
contact_point.material_mask_b = MaterialMaskData(masks_b.material_mask);
|
||||
storeToVec3(&contact_point.position, point.getContact().getPosition());
|
||||
storeToVec3(&contact_point.separating_normal, point.getContact().getSeparatingNormal());
|
||||
|
||||
float distance = point.getContact().getDistance();
|
||||
if (distance < 0 && bool(clamp_distance))
|
||||
distance = 0;
|
||||
|
||||
contact_point.separating_distance = distance;
|
||||
contact_point.shape_key_a = point.m_shapeKeyA;
|
||||
contact_point.shape_key_b = point.m_shapeKeyB;
|
||||
contact_point.separating_normal *= -1;
|
||||
|
||||
return System::instance()->getContactMgr()->registerContactPoint(mContactPointInfo,
|
||||
contact_point, true);
|
||||
}
|
||||
|
||||
FilteredClosestCdPointCollector::FilteredClosestCdPointCollector(
|
||||
RigidBody* body, QueryContactPointInfo* contact_point_info)
|
||||
: mBody(body), mContactPointInfo(contact_point_info) {}
|
||||
|
||||
// NON_MATCHING: the original code has a useless `if (this == nullptr)` check for some reason
|
||||
FilteredClosestCdPointCollector::~FilteredClosestCdPointCollector() = default;
|
||||
|
||||
void FilteredClosestCdPointCollector::addCdPoint(const hkpCdPoint& point) {
|
||||
auto* body = mBody;
|
||||
const auto point_distance = point.getContact().getDistanceSimdReal();
|
||||
|
||||
if (hasHit() && !point_distance.isLess(getHitContact().getDistanceSimdReal()))
|
||||
return;
|
||||
|
||||
auto* hit_entity = getHkpEntity(*point.m_cdBodyB.getRootCollidable());
|
||||
if (!hit_entity)
|
||||
return;
|
||||
|
||||
auto* hit_body = getRigidBody(hit_entity);
|
||||
|
||||
if (System::instance()->getEntityContactListenerField91() && hit_body->isEntity() &&
|
||||
EntityContactListener::isObjectOrGroundOrNPCOrTree(*hit_body)) {
|
||||
return;
|
||||
}
|
||||
|
||||
RigidBodyCollisionMasks masks_a, masks_b;
|
||||
sead::Vector3f contact_position;
|
||||
storeToVec3(&contact_position, point.getContact().getPosition());
|
||||
auto shape_key_a = point.m_cdBodyA.getShapeKey();
|
||||
auto shape_key_b = point.m_cdBodyB.getShapeKey();
|
||||
|
||||
body->getCollisionMasks(&masks_a, &shape_key_a, contact_position);
|
||||
hit_body->getCollisionMasks(&masks_b, &shape_key_b, contact_position);
|
||||
|
||||
auto* filter = System::instance()->getGroupFilter(hit_body->getLayerType());
|
||||
auto layer_b = filter->getCollisionFilterInfoLayer(masks_b.collision_filter_info);
|
||||
|
||||
if (!mContactPointInfo->isLayerSubscribed(layer_b))
|
||||
return;
|
||||
|
||||
m_hitPoint.setContact(point.getContact());
|
||||
m_hitPoint.m_rootCollidableA = point.m_cdBodyA.getRootCollidable();
|
||||
m_hitPoint.m_shapeKeyA = point.m_cdBodyA.getShapeKey();
|
||||
m_hitPoint.m_rootCollidableB = point.m_cdBodyB.getRootCollidable();
|
||||
m_hitPoint.m_shapeKeyB = point.m_cdBodyB.getShapeKey();
|
||||
// Set m_earlyOutDistance to the current distance as an optimisation.
|
||||
m_earlyOutDistance = point_distance;
|
||||
m_unweldedNormal = point.getUnweldedNormal();
|
||||
}
|
||||
|
||||
void FilteredClosestCdPointCollector::reset() {
|
||||
hkpClosestCdPointCollector::reset();
|
||||
m_earlyOutDistance = 1.0f;
|
||||
}
|
||||
|
||||
} // namespace ksys::phys
|
||||
Reference in New Issue
Block a user