From 1531f9b628671ffe850555d62d18f67c88b7ff3a Mon Sep 17 00:00:00 2001 From: Justin Marshall Date: Sun, 9 Aug 2026 05:16:51 -0700 Subject: [PATCH] idTech 5 physics code. --- .../engine/gamelib/physics/afconstraint.cpp | 1169 +++++++++++++++ source/engine/gamelib/physics/afconstraint.h | 657 +++++---- source/engine/gamelib/physics/physics_actor.h | 7 +- source/engine/gamelib/physics/physics_af.cpp | 1281 +++++++++++++++++ source/engine/gamelib/physics/physics_af.h | 372 +++-- source/engine/gamelib/physics/physics_ai.cpp | 449 ++++++ source/engine/gamelib/physics/physics_ai.h | 208 ++- .../gamelib/physics/physics_dynamicbase.h | 2 +- .../gamelib/physics/physics_parametric.cpp | 923 ++++++++++++ .../gamelib/physics/physics_parametric.h | 452 +++--- .../engine/gamelib/physics/physics_player.cpp | 978 +++++++++++++ .../engine/gamelib/physics/physics_player.h | 442 +++--- .../gamelib/physics/physics_rigidbody.cpp | 606 ++++++++ .../gamelib/physics/physics_rigidbody.h | 233 +-- .../gamelib/physics/physics_rigidbody3dof.cpp | 517 +++++++ .../gamelib/physics/physics_rigidbody3dof.h | 256 ++-- .../gamelib/physics/physics_staticmulti.cpp | 509 +++++++ .../gamelib/physics/physics_staticmulti.h | 185 +-- 18 files changed, 7995 insertions(+), 1251 deletions(-) create mode 100644 source/engine/gamelib/physics/afconstraint.cpp create mode 100644 source/engine/gamelib/physics/physics_af.cpp create mode 100644 source/engine/gamelib/physics/physics_ai.cpp create mode 100644 source/engine/gamelib/physics/physics_parametric.cpp create mode 100644 source/engine/gamelib/physics/physics_player.cpp create mode 100644 source/engine/gamelib/physics/physics_rigidbody.cpp create mode 100644 source/engine/gamelib/physics/physics_rigidbody3dof.cpp create mode 100644 source/engine/gamelib/physics/physics_staticmulti.cpp diff --git a/source/engine/gamelib/physics/afconstraint.cpp b/source/engine/gamelib/physics/afconstraint.cpp new file mode 100644 index 0000000..2b17701 --- /dev/null +++ b/source/engine/gamelib/physics/afconstraint.cpp @@ -0,0 +1,1169 @@ +#include "gamelib/physics/afconstraint.h" + +#include "gamelib/physics/clipmodel.h" +#include "idlib/math/rotation.h" + +#include +#include +#include +#include + +void GameLib_SerializeAFConstraint(idSerializer* serializer, + idAFConstraint& constraint); +void GameLib_DrawAFConstraint(const idAFConstraint& constraint, + const idVec3& center); +void GameLib_AddAFFrameConstraint(idPhysics_AF* physics, + idAFConstraint* constraint); +float GameLib_GetAFJointFrictionScale(const idPhysics_AF* physics); +float GameLib_GetAFContactFrictionScale(const idPhysics_AF* physics); +void GameLib_SerializeAFSuspension(idSerializer* serializer, + idAFConstraint_Suspension& suspension); + +namespace { + +constexpr float PI = 3.14159265358979323846f; +constexpr float DEG2RAD = PI / 180.0f; +constexpr int ENTITYNUM_NONE = 0x1FFF; +const idVec3 kZeroVector(0.0f, 0.0f, 0.0f); + +void ProjectOntoPlane(idVec3& vector, const idVec3& normal) { + vector = vector - normal * vector.Dot(normal); +} + +idVec3 BodyOrigin(const idAFBody* const body) { + if (body == nullptr) return idVec3(0.0f, 0.0f, 0.0f); + return body->clipModel != nullptr ? body->clipModel->GetOrigin() + : body->current.worldOrigin; +} + +idMat3 BodyAxis(const idAFBody* const body) { + if (body == nullptr) return idMat3(1.0f); + return body->clipModel != nullptr ? body->clipModel->GetAxis() + : body->current.worldAxis; +} + +idVec3 WorldPoint(const idAFBody* const body, const idVec3& local) { + return body != nullptr ? BodyOrigin(body) + BodyAxis(body) * local + : local; +} + +idVec3 LocalPoint(const idAFBody* const body, const idVec3& world) { + return body != nullptr + ? BodyAxis(body).Transpose() * (world - BodyOrigin(body)) : world; +} + +idVec3 WorldAxis(const idAFBody* const body, const idVec3& local) { + return body != nullptr ? BodyAxis(body) * local : local; +} + +idVec3 LocalAxis(const idAFBody* const body, const idVec3& world) { + return body != nullptr ? BodyAxis(body).Transpose() * world : world; +} + +void RegisterConstraint(idAFBody* const body, idAFConstraint* const c) { + if (body == nullptr || c == nullptr) return; + for (int i = 0; i < body->constraints.Num(); ++i) + if (body->constraints[i] == c) return; + body->constraints.Append(c); +} + +void UnregisterConstraint(idAFBody* const body, idAFConstraint* const c) { + if (body == nullptr) return; + body->constraints.Remove(c); + if (body->primaryConstraint == c) body->primaryConstraint = nullptr; +} + +void SetSpatialRow(idSpatialMat& matrix, const int row, + const idVec3& linear, const idVec3& angular) { + matrix(row, 0) = linear.x; + matrix(row, 1) = linear.y; + matrix(row, 2) = linear.z; + matrix(row, 3) = angular.x; + matrix(row, 4) = angular.y; + matrix(row, 5) = angular.z; +} + +void SetPointRow(idSpatialMat& matrix, const int row, + const idAFBody* const body, const idVec3& point, + const idVec3& direction, const float sign) { + const idVec3 linear = direction * sign; + const idVec3 angular = (point - BodyOrigin(body)).Cross(direction) + * sign; + SetSpatialRow(matrix, row, linear, angular); +} + +void BasisForNormal(const idVec3& normal, idVec3& tangent1, + idVec3& tangent2) { + const idVec3 reference = std::fabs(normal.z) < 0.7f + ? idVec3(0.0f, 0.0f, 1.0f) : idVec3(1.0f, 0.0f, 0.0f); + tangent1 = normal.Cross(reference); + tangent1.NormalizeFast(); + tangent2 = normal.Cross(tangent1); + tangent2.NormalizeFast(); +} + +float ClampUnit(const float value) { + return (std::max)(-1.0f, (std::min)(1.0f, value)); +} + +float SafeInverseTime(const float timeStep) { + return timeStep > 1.0e-6f ? 1.0f / timeStep : 0.0f; +} + +} // namespace + +idAFConstraint::idAFConstraint(const char* const constraintName, + const constraintType_t constraintType, idAFBody* const firstBody, + idAFBody* const secondBody) + : type(constraintType) + , name() + , body1(nullptr) + , body2(nullptr) + , physics(nullptr) + , lm() + , J1() + , J2() + , c1() + , c2() + , lo() + , hi() + , e() + , boxConstraint(nullptr) + , boxIndex{} + , boxScale{} + , invI() + , J() + , firstIndex(0) + , fl{} { + name.Set(constraintName != nullptr ? constraintName : "noname"); + for (int i = 0; i < 6; ++i) { + lo[i] = -1.0e20f; + hi[i] = 1.0e20f; + e[i] = 0.0f; + boxIndex[i] = -1; + boxScale[i] = 1.0f; + } + fl.allowPrimary = 1; + SetBody1(firstBody); + SetBody2(secondBody); +} + +idAFConstraint::~idAFConstraint() { + UnregisterConstraint(body1, this); + UnregisterConstraint(body2, this); +} + +void* idAFConstraint::operator new(const std::size_t size) { +#if defined(_MSC_VER) + return _aligned_malloc(size, 16); +#else + const std::size_t alignedSize = (size + 15u) & ~std::size_t(15u); + return std::aligned_alloc(16, alignedSize); +#endif +} + +void idAFConstraint::operator delete(void* const pointer) noexcept { +#if defined(_MSC_VER) + _aligned_free(pointer); +#else + std::free(pointer); +#endif +} + +void idAFConstraint::SetBody1(idAFBody* const body) { + if (body1 == body) return; + UnregisterConstraint(body1, this); + body1 = body; + RegisterConstraint(body1, this); +} + +void idAFConstraint::SetBody2(idAFBody* const body) { + if (body2 == body) return; + UnregisterConstraint(body2, this); + body2 = body; + RegisterConstraint(body2, this); +} + +void idAFConstraint::DebugDraw() { + idVec3 center; + GetCenter(center); + GameLib_DrawAFConstraint(*this, center); +} +void idAFConstraint::Translate(const idVec3&) {} +void idAFConstraint::Rotate(const idRotation&) {} +void idAFConstraint::GetCenter(idVec3& center) const { + if (body1 != nullptr && body2 != nullptr) + center = (BodyOrigin(body1) + BodyOrigin(body2)) * 0.5f; + else if (body1 != nullptr) center = BodyOrigin(body1); + else if (body2 != nullptr) center = BodyOrigin(body2); + else center.Zero(); +} +void idAFConstraint::IssueCollisionQueries() {} +void idAFConstraint::CancelCollisionQueries() {} +void idAFConstraint::Serialize(idSerializer* const serializer) { + if (serializer != nullptr) GameLib_SerializeAFConstraint(serializer, *this); +} +void idAFConstraint::Evaluate(const idPhysics_AF*, float) { + J1.Zero(); + J2.Zero(); + c1.Zero(); + c2.Zero(); +} +void idAFConstraint::ApplyFriction(float) {} + +void idAFConstraint::InitSize(const int size) { + const int rows = (std::max)(0, (std::min)(6, size)); + J1.Zero(rows, 6); + J2.Zero(rows, 6); + invI.Zero(rows, rows); + J.Zero(rows, rows); + lm.Zero(); + c1.Zero(); + c2.Zero(); + lo.Zero(); + hi.Zero(); + e.Zero(); + for (int i = 0; i < 6; ++i) { + lo[i] = i < rows ? -1.0e20f : 0.0f; + hi[i] = i < rows ? 1.0e20f : 0.0f; + boxIndex[i] = -1; + boxScale[i] = 1.0f; + } +} + +idAFConstraint_Fixed::idAFConstraint_Fixed(const char* const fixedName, + idAFBody* const firstBody, idAFBody* const secondBody) + : idAFConstraint(fixedName, CONSTRAINT_FIXED, firstBody, secondBody) + , offset(0.0f, 0.0f, 0.0f) + , relAxis(1.0f) + , shouldClamp(false) { + InitSize(6); + InitOffset(); +} + +void idAFConstraint_Fixed::InitOffset() { + if (body1 == nullptr) return; + offset = LocalPoint(body1, BodyOrigin(body2)); + relAxis = BodyAxis(body1).Transpose() * BodyAxis(body2); +} +void idAFConstraint_Fixed::SetBody1(idAFBody* const body) { + idAFConstraint::SetBody1(body); + InitOffset(); +} +void idAFConstraint_Fixed::SetBody2(idAFBody* const body) { + idAFConstraint::SetBody2(body); + InitOffset(); +} +void idAFConstraint_Fixed::Rotate(const idRotation& rotation) { + if (body2 == nullptr) offset *= rotation; +} +void idAFConstraint_Fixed::GetCenter(idVec3& center) const { + center = body1 != nullptr ? WorldPoint(body1, offset) + : BodyOrigin(body2); +} +void idAFConstraint_Fixed::DebugDraw() { idAFConstraint::DebugDraw(); } +void idAFConstraint_Fixed::Evaluate(const idPhysics_AF*, + const float invTimeStep) { + InitSize(6); + const idVec3 p1 = WorldPoint(body1, offset); + const idVec3 p2 = BodyOrigin(body2); + const idVec3 delta = p2 - p1; + const idVec3 axes[3] = {idVec3(1.0f, 0.0f, 0.0f), + idVec3(0.0f, 1.0f, 0.0f), idVec3(0.0f, 0.0f, 1.0f)}; + for (int row = 0; row < 3; ++row) { + SetPointRow(J1, row, body1, p1, axes[row], -1.0f); + if (body2 != nullptr) + SetPointRow(J2, row, body2, p2, axes[row], 1.0f); + c1[row] = delta.Dot(axes[row]) * invTimeStep; + } + const idMat3 target = BodyAxis(body1) * relAxis; + const idMat3 actual = BodyAxis(body2); + const idVec3 angularError = (target[0].Cross(actual[0]) + + target[1].Cross(actual[1]) + target[2].Cross(actual[2])) * 0.5f; + for (int row = 0; row < 3; ++row) { + SetSpatialRow(J1, row + 3, idVec3(0.0f, 0.0f, 0.0f), + axes[row] * -1.0f); + if (body2 != nullptr) + SetSpatialRow(J2, row + 3, idVec3(0.0f, 0.0f, 0.0f), + axes[row]); + c1[row + 3] = angularError.Dot(axes[row]) * invTimeStep; + } +} + +idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint( + const char* const jointName, idAFBody* const firstBody, + idAFBody* const secondBody) + : idAFConstraint(jointName, CONSTRAINT_BALLANDSOCKETJOINT, + firstBody, secondBody) + , anchor1(0.0f, 0.0f, 0.0f) + , anchor2(0.0f, 0.0f, 0.0f) + , friction(0.0f) + , coneLimit(nullptr) + , pyramidLimit(nullptr) + , fc(nullptr) { + InitSize(3); +} + +idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint() { + delete coneLimit; + delete pyramidLimit; + delete fc; +} +void idAFConstraint_BallAndSocketJoint::SetNoLimit() { + delete coneLimit; coneLimit = nullptr; + delete pyramidLimit; pyramidLimit = nullptr; +} +void idAFConstraint_BallAndSocketJoint::SetWorldPosition( + const idVec3& position) { SetAnchor(position); } +float idAFConstraint_BallAndSocketJoint::GetFriction() const { + const float scale = physics != nullptr + ? GameLib_GetAFJointFrictionScale(physics) : 1.0f; + return (std::max)(0.0f, friction * scale); +} +void idAFConstraint_BallAndSocketJoint::SetAnchor( + const idVec3& position) { + anchor1 = LocalPoint(body1, position); + anchor2 = LocalPoint(body2, position); + if (coneLimit != nullptr) coneLimit->coneAnchor = anchor2; + if (pyramidLimit != nullptr) pyramidLimit->pyramidAnchor = anchor2; +} +void idAFConstraint_BallAndSocketJoint::Translate( + const idVec3& translation) { + if (body2 == nullptr) anchor2 = anchor2 + translation; +} +void idAFConstraint_BallAndSocketJoint::Rotate( + const idRotation& rotation) { + if (body2 == nullptr) anchor2 *= rotation; + if (coneLimit != nullptr) coneLimit->Rotate(rotation); + if (pyramidLimit != nullptr) pyramidLimit->Rotate(rotation); +} +void idAFConstraint_BallAndSocketJoint::GetCenter(idVec3& center) const { + center = WorldPoint(body1, anchor1); +} +void idAFConstraint_BallAndSocketJoint::DebugDraw() { + idAFConstraint::DebugDraw(); + if (coneLimit != nullptr) coneLimit->DebugDraw(); + if (pyramidLimit != nullptr) pyramidLimit->DebugDraw(); +} +void idAFConstraint_BallAndSocketJoint::Evaluate(const idPhysics_AF*, + const float invTimeStep) { + InitSize(3); + const idVec3 p1 = WorldPoint(body1, anchor1); + const idVec3 p2 = WorldPoint(body2, anchor2); + const idVec3 delta = p2 - p1; + const idVec3 axes[3] = {idVec3(1.0f, 0.0f, 0.0f), + idVec3(0.0f, 1.0f, 0.0f), idVec3(0.0f, 0.0f, 1.0f)}; + for (int row = 0; row < 3; ++row) { + SetPointRow(J1, row, body1, p1, axes[row], -1.0f); + if (body2 != nullptr) + SetPointRow(J2, row, body2, p2, axes[row], 1.0f); + c1[row] = delta.Dot(axes[row]) * invTimeStep; + } + if (coneLimit != nullptr) coneLimit->Add(physics, invTimeStep); + if (pyramidLimit != nullptr) pyramidLimit->Add(physics, invTimeStep); +} +void idAFConstraint_BallAndSocketJoint::SetConeLimit( + const idVec3& axis, const float angle, const idVec3& bodyAxis) { + delete pyramidLimit; pyramidLimit = nullptr; + if (coneLimit == nullptr) coneLimit = new idAFConstraint_ConeLimit(); + coneLimit->physics = physics; + coneLimit->Setup(body1, body2, anchor2, axis, angle, bodyAxis); +} +void idAFConstraint_BallAndSocketJoint::SetPyramidLimit( + const idVec3& axis, const idVec3& baseAxis, const float angle1, + const float angle2, const idVec3& bodyAxis) { + delete coneLimit; coneLimit = nullptr; + if (pyramidLimit == nullptr) + pyramidLimit = new idAFConstraint_PyramidLimit(); + pyramidLimit->physics = physics; + pyramidLimit->Setup(body1, body2, anchor2, axis, baseAxis, + angle1, angle2, bodyAxis); +} +void idAFConstraint_BallAndSocketJoint::ApplyFriction( + const float invTimeStep) { + if (GetFriction() <= 0.0f) return; + if (fc == nullptr) fc = new idAFConstraint_BallAndSocketJointFriction(this); + fc->Add(physics, invTimeStep); +} + +idAFConstraint_BallAndSocketJointFriction:: +idAFConstraint_BallAndSocketJointFriction( + idAFConstraint_BallAndSocketJoint* const sourceJoint) + : idAFConstraint("ballAndSocketFriction", CONSTRAINT_FRICTION, + sourceJoint != nullptr ? sourceJoint->body1 : nullptr, + sourceJoint != nullptr ? sourceJoint->body2 : nullptr) + , joint(sourceJoint) { + InitSize(3); + fl.frameConstraint = 1; + fl.allowPrimary = 0; +} +bool idAFConstraint_BallAndSocketJointFriction::Add( + idPhysics_AF* const af, float) { + if (joint == nullptr || joint->GetFriction() <= 0.0f) return false; + physics = af; + InitSize(3); + const idVec3 axes[3] = {idVec3(1.0f, 0.0f, 0.0f), + idVec3(0.0f, 1.0f, 0.0f), idVec3(0.0f, 0.0f, 1.0f)}; + for (int row = 0; row < 3; ++row) { + SetSpatialRow(J1, row, idVec3(0.0f, 0.0f, 0.0f), axes[row]); + if (body2 != nullptr) + SetSpatialRow(J2, row, idVec3(0.0f, 0.0f, 0.0f), + axes[row] * -1.0f); + lo[row] = -joint->GetFriction(); + hi[row] = joint->GetFriction(); + } + GameLib_AddAFFrameConstraint(af, this); + return true; +} + +idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint( + const char* const jointName, idAFBody* const firstBody, + idAFBody* const secondBody) + : idAFConstraint(jointName, CONSTRAINT_UNIVERSALJOINT, + firstBody, secondBody) + , anchor1(0.0f, 0.0f, 0.0f) + , anchor2(0.0f, 0.0f, 0.0f) + , shaft1(1.0f, 0.0f, 0.0f) + , shaft2(0.0f, 1.0f, 0.0f) + , axis1(0.0f, 1.0f, 0.0f) + , axis2(1.0f, 0.0f, 0.0f) + , friction(0.0f) + , coneLimit(nullptr) + , pyramidLimit(nullptr) + , fc(nullptr) { InitSize(4); } + +idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint() { + delete coneLimit; + delete pyramidLimit; + delete fc; +} +void idAFConstraint_UniversalJoint::SetNoLimit() { + delete coneLimit; coneLimit = nullptr; + delete pyramidLimit; pyramidLimit = nullptr; +} +float idAFConstraint_UniversalJoint::GetFriction() const { + const float scale = physics != nullptr + ? GameLib_GetAFJointFrictionScale(physics) : 1.0f; + return (std::max)(0.0f, friction * scale); +} +void idAFConstraint_UniversalJoint::SetAnchor(const idVec3& position) { + anchor1 = LocalPoint(body1, position); + anchor2 = LocalPoint(body2, position); + if (coneLimit != nullptr) coneLimit->coneAnchor = anchor2; + if (pyramidLimit != nullptr) pyramidLimit->pyramidAnchor = anchor2; +} +void idAFConstraint_UniversalJoint::SetShafts(const idVec3& first, + const idVec3& second) { + idVec3 normalizedFirst = first; + idVec3 normalizedSecond = second; + normalizedFirst.NormalizeFast(); + normalizedSecond.NormalizeFast(); + shaft1 = LocalAxis(body1, normalizedFirst); + shaft2 = LocalAxis(body2, normalizedSecond); + axis1 = LocalAxis(body1, normalizedSecond); + axis2 = LocalAxis(body2, normalizedFirst); +} +void idAFConstraint_UniversalJoint::Translate(const idVec3& translation) { + if (body2 == nullptr) anchor2 = anchor2 + translation; +} +void idAFConstraint_UniversalJoint::Rotate(const idRotation& rotation) { + if (body2 == nullptr) { + anchor2 *= rotation; + shaft2 = rotation.ToMat3() * shaft2; + axis2 = rotation.ToMat3() * axis2; + } + if (coneLimit != nullptr) coneLimit->Rotate(rotation); + if (pyramidLimit != nullptr) pyramidLimit->Rotate(rotation); +} +void idAFConstraint_UniversalJoint::GetCenter(idVec3& center) const { + center = WorldPoint(body1, anchor1); +} +void idAFConstraint_UniversalJoint::DebugDraw() { + idAFConstraint::DebugDraw(); + if (coneLimit != nullptr) coneLimit->DebugDraw(); + if (pyramidLimit != nullptr) pyramidLimit->DebugDraw(); +} +void idAFConstraint_UniversalJoint::Evaluate(const idPhysics_AF*, + const float invTimeStep) { + InitSize(4); + const idVec3 p1 = WorldPoint(body1, anchor1); + const idVec3 p2 = WorldPoint(body2, anchor2); + const idVec3 delta = p2 - p1; + const idVec3 axes[3] = {idVec3(1.0f, 0.0f, 0.0f), + idVec3(0.0f, 1.0f, 0.0f), idVec3(0.0f, 0.0f, 1.0f)}; + for (int row = 0; row < 3; ++row) { + SetPointRow(J1, row, body1, p1, axes[row], -1.0f); + if (body2 != nullptr) + SetPointRow(J2, row, body2, p2, axes[row], 1.0f); + c1[row] = delta.Dot(axes[row]) * invTimeStep; + } + const idVec3 worldShaft1 = WorldAxis(body1, shaft1); + const idVec3 worldShaft2 = WorldAxis(body2, shaft2); + const idVec3 angular = worldShaft1.Cross(worldShaft2); + SetSpatialRow(J1, 3, idVec3(0.0f, 0.0f, 0.0f), angular); + if (body2 != nullptr) + SetSpatialRow(J2, 3, idVec3(0.0f, 0.0f, 0.0f), -angular); + c1[3] = worldShaft1.Dot(worldShaft2) * invTimeStep; + if (coneLimit != nullptr) coneLimit->Add(physics, invTimeStep); + if (pyramidLimit != nullptr) pyramidLimit->Add(physics, invTimeStep); +} +void idAFConstraint_UniversalJoint::SetConeLimit(const idVec3& axis, + const float angle, const idVec3& bodyAxis) { + delete pyramidLimit; pyramidLimit = nullptr; + if (coneLimit == nullptr) coneLimit = new idAFConstraint_ConeLimit(); + coneLimit->physics = physics; + coneLimit->Setup(body1, body2, anchor2, axis, angle, bodyAxis); +} +void idAFConstraint_UniversalJoint::SetPyramidLimit( + const idVec3& axis, const idVec3& baseAxis, const float angle1, + const float angle2, const idVec3& bodyAxis) { + delete coneLimit; coneLimit = nullptr; + if (pyramidLimit == nullptr) + pyramidLimit = new idAFConstraint_PyramidLimit(); + pyramidLimit->physics = physics; + pyramidLimit->Setup(body1, body2, anchor2, axis, baseAxis, + angle1, angle2, bodyAxis); +} +void idAFConstraint_UniversalJoint::ApplyFriction( + const float invTimeStep) { + if (GetFriction() <= 0.0f) return; + if (fc == nullptr) fc = new idAFConstraint_UniversalJointFriction(this); + fc->Add(physics, invTimeStep); +} + +idAFConstraint_UniversalJointFriction:: +idAFConstraint_UniversalJointFriction( + idAFConstraint_UniversalJoint* const sourceJoint) + : idAFConstraint("universalFriction", CONSTRAINT_FRICTION, + sourceJoint != nullptr ? sourceJoint->body1 : nullptr, + sourceJoint != nullptr ? sourceJoint->body2 : nullptr) + , joint(sourceJoint) { + InitSize(2); + fl.frameConstraint = 1; + fl.allowPrimary = 0; +} +bool idAFConstraint_UniversalJointFriction::Add(idPhysics_AF* const af, + float) { + if (joint == nullptr || joint->GetFriction() <= 0.0f) return false; + physics = af; + Evaluate(af, 0.0f); + for (int row = 0; row < 2; ++row) { + lo[row] = -joint->GetFriction(); + hi[row] = joint->GetFriction(); + } + GameLib_AddAFFrameConstraint(af, this); + return true; +} +void idAFConstraint_UniversalJointFriction::Evaluate( + const idPhysics_AF*, float) { + InitSize(2); + if (joint == nullptr) return; + idVec3 first = WorldAxis(body1, joint->shaft1); + idVec3 second = WorldAxis(body2, joint->shaft2); + idVec3 cross = first.Cross(second); + if (cross.NormalizeFast() == 0.0f) BasisForNormal(first, cross, second); + const idVec3 axes[2] = {first, second}; + for (int row = 0; row < 2; ++row) { + SetSpatialRow(J1, row, idVec3(0.0f, 0.0f, 0.0f), axes[row]); + if (body2 != nullptr) + SetSpatialRow(J2, row, idVec3(0.0f, 0.0f, 0.0f), -axes[row]); + } +} + +idAFConstraint_Hinge::idAFConstraint_Hinge(const char* const hingeName, + idAFBody* const firstBody, idAFBody* const secondBody) + : idAFConstraint(hingeName, CONSTRAINT_HINGE, firstBody, secondBody) + , anchor1(0.0f, 0.0f, 0.0f) + , anchor2(0.0f, 0.0f, 0.0f) + , axis1(0.0f, 0.0f, 1.0f) + , axis2(0.0f, 0.0f, 1.0f) + , initialAxis(1.0f) + , friction(0.0f) + , coneLimit(nullptr) + , steering(nullptr) + , fc(nullptr) { InitSize(5); } + +idAFConstraint_Hinge::~idAFConstraint_Hinge() { + delete coneLimit; + delete steering; + delete fc; +} +void idAFConstraint_Hinge::SetNoLimit() { + delete coneLimit; coneLimit = nullptr; +} +void idAFConstraint_Hinge::SetAnchor(const idVec3& position) { + anchor1 = LocalPoint(body1, position); + anchor2 = LocalPoint(body2, position); +} +void idAFConstraint_Hinge::SetAxis(const idVec3& sourceAxis) { + idVec3 normalized = sourceAxis; + normalized.NormalizeFast(); + axis1 = LocalAxis(body1, normalized); + axis2 = LocalAxis(body2, normalized); + initialAxis = BodyAxis(body1).Transpose() * BodyAxis(body2); +} +float idAFConstraint_Hinge::GetFriction() const { + const float scale = physics != nullptr + ? GameLib_GetAFJointFrictionScale(physics) : 1.0f; + return (std::max)(0.0f, friction * scale); +} +float idAFConstraint_Hinge::GetAngle() const { + const idVec3 hingeAxis = WorldAxis(body1, axis1); + const idMat3 relative = BodyAxis(body1).Transpose() * BodyAxis(body2); + idVec3 reference = initialAxis[0]; + idVec3 currentReference = relative[0]; + ProjectOntoPlane(reference, axis1); + ProjectOntoPlane(currentReference, axis1); + reference.NormalizeFast(); + currentReference.NormalizeFast(); + return std::atan2(hingeAxis.Dot(reference.Cross(currentReference)), + ClampUnit(reference.Dot(currentReference))) / DEG2RAD; +} +void idAFConstraint_Hinge::Translate(const idVec3& translation) { + if (body2 == nullptr) anchor2 = anchor2 + translation; +} +void idAFConstraint_Hinge::Rotate(const idRotation& rotation) { + if (body2 == nullptr) { + anchor2 *= rotation; + axis2 = rotation.ToMat3() * axis2; + } + if (coneLimit != nullptr) coneLimit->Rotate(rotation); +} +void idAFConstraint_Hinge::GetCenter(idVec3& center) const { + center = WorldPoint(body1, anchor1); +} +void idAFConstraint_Hinge::DebugDraw() { + idAFConstraint::DebugDraw(); + if (coneLimit != nullptr) coneLimit->DebugDraw(); +} +void idAFConstraint_Hinge::Evaluate(const idPhysics_AF*, + const float invTimeStep) { + InitSize(5); + const idVec3 p1 = WorldPoint(body1, anchor1); + const idVec3 p2 = WorldPoint(body2, anchor2); + const idVec3 delta = p2 - p1; + const idVec3 axes[3] = {idVec3(1.0f, 0.0f, 0.0f), + idVec3(0.0f, 1.0f, 0.0f), idVec3(0.0f, 0.0f, 1.0f)}; + for (int row = 0; row < 3; ++row) { + SetPointRow(J1, row, body1, p1, axes[row], -1.0f); + if (body2 != nullptr) + SetPointRow(J2, row, body2, p2, axes[row], 1.0f); + c1[row] = delta.Dot(axes[row]) * invTimeStep; + } + const idVec3 a1 = WorldAxis(body1, axis1); + const idVec3 a2 = WorldAxis(body2, axis2); + idVec3 tangent1; + idVec3 tangent2; + BasisForNormal(a1, tangent1, tangent2); + const idVec3 tangents[2] = {tangent1, tangent2}; + for (int row = 0; row < 2; ++row) { + SetSpatialRow(J1, row + 3, kZeroVector, tangents[row]); + if (body2 != nullptr) + SetSpatialRow(J2, row + 3, kZeroVector, -tangents[row]); + c1[row + 3] = a2.Dot(tangents[row]) * invTimeStep; + } + if (coneLimit != nullptr) coneLimit->Add(physics, invTimeStep); + if (steering != nullptr) steering->Add(physics, invTimeStep); +} +void idAFConstraint_Hinge::SetLimit(const float angle1, + const float angle2, const float limitEpsilon) { + if (coneLimit == nullptr) coneLimit = new idAFConstraint_ConeLimit(); + idVec3 normal; + idVec3 base; + BasisForNormal(WorldAxis(body1, axis1), normal, base); + coneLimit->physics = physics; + coneLimit->Setup(body1, body2, anchor2, normal, + (std::max)(std::fabs(angle1), std::fabs(angle2)), base); + coneLimit->epsilon = limitEpsilon; +} +void idAFConstraint_Hinge::ApplyFriction(const float invTimeStep) { + if (GetFriction() <= 0.0f) return; + if (fc == nullptr) fc = new idAFConstraint_HingeFriction(this); + fc->Add(physics, invTimeStep); +} + +idAFConstraint_HingeFriction::idAFConstraint_HingeFriction( + idAFConstraint_Hinge* const sourceHinge) + : idAFConstraint("hingeFriction", CONSTRAINT_FRICTION, + sourceHinge != nullptr ? sourceHinge->body1 : nullptr, + sourceHinge != nullptr ? sourceHinge->body2 : nullptr) + , hinge(sourceHinge) { + InitSize(1); + fl.frameConstraint = 1; + fl.allowPrimary = 0; +} +bool idAFConstraint_HingeFriction::Add(idPhysics_AF* const af, float) { + if (hinge == nullptr || hinge->GetFriction() <= 0.0f) return false; + physics = af; + InitSize(1); + const idVec3 axis = WorldAxis(body1, hinge->axis1); + SetSpatialRow(J1, 0, kZeroVector, axis); + if (body2 != nullptr) SetSpatialRow(J2, 0, kZeroVector, -axis); + lo[0] = -hinge->GetFriction(); + hi[0] = hinge->GetFriction(); + GameLib_AddAFFrameConstraint(af, this); + return true; +} + +idAFConstraint_HingeSteering::idAFConstraint_HingeSteering( + idAFConstraint_Hinge* const sourceHinge) + : idAFConstraint("hingeSteering", CONSTRAINT_HINGESTEERING, + sourceHinge != nullptr ? sourceHinge->body1 : nullptr, + sourceHinge != nullptr ? sourceHinge->body2 : nullptr) + , hinge(sourceHinge) + , steerAngle(0.0f) + , steerSpeed(0.0f) + , epsilon(0.001f) { + InitSize(1); + fl.frameConstraint = 1; + fl.allowPrimary = 0; +} +bool idAFConstraint_HingeSteering::Add(idPhysics_AF* const af, + const float invTimeStep) { + if (hinge == nullptr) return false; + physics = af; + InitSize(1); + const idVec3 axis = WorldAxis(body1, hinge->axis1); + SetSpatialRow(J1, 0, kZeroVector, axis); + if (body2 != nullptr) SetSpatialRow(J2, 0, kZeroVector, -axis); + const float delta = steerAngle - hinge->GetAngle(); + c1[0] = (std::max)(-steerSpeed, (std::min)(steerSpeed, + delta * invTimeStep)); + GameLib_AddAFFrameConstraint(af, this); + return std::fabs(delta) > epsilon; +} + +idAFConstraint_Slider::idAFConstraint_Slider(const char* const sliderName, + idAFBody* const firstBody, idAFBody* const secondBody) + : idAFConstraint(sliderName, CONSTRAINT_SLIDER, firstBody, secondBody) + , axis(1.0f, 0.0f, 0.0f) + , offset(0.0f, 0.0f, 0.0f) + , relAxis(1.0f) { InitSize(5); } +void idAFConstraint_Slider::SetAxis(const idVec3& sourceAxis) { + axis = sourceAxis; + axis.NormalizeFast(); + if (body1 != nullptr) axis = LocalAxis(body1, axis); + offset = LocalPoint(body1, BodyOrigin(body2)); + relAxis = BodyAxis(body1).Transpose() * BodyAxis(body2); +} +void idAFConstraint_Slider::GetCenter(idVec3& center) const { + center = (BodyOrigin(body1) + BodyOrigin(body2)) * 0.5f; +} +void idAFConstraint_Slider::DebugDraw() { idAFConstraint::DebugDraw(); } +void idAFConstraint_Slider::Evaluate(const idPhysics_AF*, + const float invTimeStep) { + InitSize(5); + const idVec3 worldAxis = WorldAxis(body1, axis); + idVec3 tangent1; + idVec3 tangent2; + BasisForNormal(worldAxis, tangent1, tangent2); + const idVec3 delta = BodyOrigin(body2) - WorldPoint(body1, offset); + const idVec3 tangents[2] = {tangent1, tangent2}; + for (int row = 0; row < 2; ++row) { + SetSpatialRow(J1, row, -tangents[row], kZeroVector); + if (body2 != nullptr) + SetSpatialRow(J2, row, tangents[row], kZeroVector); + c1[row] = delta.Dot(tangents[row]) * invTimeStep; + } + const idMat3 target = BodyAxis(body1) * relAxis; + const idMat3 actual = BodyAxis(body2); + const idVec3 error = (target[0].Cross(actual[0]) + + target[1].Cross(actual[1]) + target[2].Cross(actual[2])) * 0.5f; + const idVec3 axes[3] = {idVec3(1.0f, 0.0f, 0.0f), + idVec3(0.0f, 1.0f, 0.0f), idVec3(0.0f, 0.0f, 1.0f)}; + for (int row = 0; row < 3; ++row) { + SetSpatialRow(J1, row + 2, kZeroVector, -axes[row]); + if (body2 != nullptr) + SetSpatialRow(J2, row + 2, kZeroVector, axes[row]); + c1[row + 2] = error.Dot(axes[row]) * invTimeStep; + } +} + +idAFConstraint_Spring::idAFConstraint_Spring(const char* const springName, + idAFBody* const firstBody, idAFBody* const secondBody) + : idAFConstraint(springName, CONSTRAINT_SPRING, firstBody, secondBody) + , anchor1(0.0f, 0.0f, 0.0f) + , anchor2(0.0f, 0.0f, 0.0f) + , kstretch(100.0f) + , kcompress(100.0f) + , damping(0.0f) + , restLength(0.0f) + , minLength(0.0f) + , maxLength(0.0f) { InitSize(0); } +void idAFConstraint_Spring::SetSpring(const float stretch, + const float compress, const float damping_, const float rest) { + kstretch = stretch; + kcompress = compress; + damping = damping_; + restLength = rest; +} +void idAFConstraint_Spring::SetLimit(const float minimum, + const float maximum) { minLength = minimum; maxLength = maximum; } +void idAFConstraint_Spring::SetAnchor(const idVec3& first, + const idVec3& second) { + anchor1 = LocalPoint(body1, first); + anchor2 = LocalPoint(body2, second); +} +void idAFConstraint_Spring::Translate(const idVec3& translation) { + if (body2 == nullptr) anchor2 = anchor2 + translation; +} +void idAFConstraint_Spring::Rotate(const idRotation& rotation) { + if (body2 == nullptr) anchor2 *= rotation; +} +void idAFConstraint_Spring::GetCenter(idVec3& center) const { + center = (WorldPoint(body1, anchor1) + WorldPoint(body2, anchor2)) + * 0.5f; +} +void idAFConstraint_Spring::DebugDraw() { idAFConstraint::DebugDraw(); } +void idAFConstraint_Spring::Evaluate(const idPhysics_AF*, float) { + const idVec3 p1 = WorldPoint(body1, anchor1); + const idVec3 p2 = WorldPoint(body2, anchor2); + idVec3 direction = p2 - p1; + const float length = direction.NormalizeFast(); + if (length <= 1.0e-6f) return; + float displacement = length - restLength; + if (maxLength > minLength) { + if (length < minLength) displacement = length - minLength; + else if (length > maxLength) displacement = length - maxLength; + } + const float stiffness = displacement >= 0.0f ? kstretch : kcompress; + const idVec3 relativeVelocity = (body2 != nullptr + ? body2->GetPointVelocity(p2) : kZeroVector) + - (body1 != nullptr ? body1->GetPointVelocity(p1) : kZeroVector); + const float forceMagnitude = stiffness * displacement + + damping * relativeVelocity.Dot(direction); + const idVec3 force = direction * forceMagnitude; + if (body1 != nullptr) body1->AddForce(p1, force); + if (body2 != nullptr) body2->AddForce(p2, -force); +} + +idAFConstraint_Contact::idAFConstraint_Contact() + : idAFConstraint("contact", CONSTRAINT_CONTACT, nullptr, nullptr) + , contact{} + , separation(0.0f) + , fc(nullptr) { + InitSize(1); + fl.allowPrimary = 0; +} +idAFConstraint_Contact::~idAFConstraint_Contact() { delete fc; } +void idAFConstraint_Contact::Setup(idAFBody* const firstBody, + idAFBody* const secondBody, const contactInfo_t& info, + const float separation_, const float invTimeStep) { + SetBody1(firstBody); + SetBody2(secondBody); + contact = info; + separation = separation_; + InitSize(1); + SetPointRow(J1, 0, body1, contact.point, contact.normal, 1.0f); + if (body2 != nullptr) + SetPointRow(J2, 0, body2, contact.point, contact.normal, -1.0f); + c1[0] = (std::min)(0.0f, separation) * invTimeStep; + lo[0] = 0.0f; + hi[0] = 1.0e20f; +} +void idAFConstraint_Contact::GetCenter(idVec3& center) const { + center = contact.point; +} +void idAFConstraint_Contact::DebugDraw() { idAFConstraint::DebugDraw(); } +void idAFConstraint_Contact::ApplyFriction(const float invTimeStep) { + const float frictionScale = physics != nullptr + ? GameLib_GetAFContactFrictionScale(physics) : 1.0f; + if (frictionScale <= 0.0f || body1 == nullptr) return; + if (fc == nullptr) fc = new idAFConstraint_ContactFriction(this); + fc->Add(physics, invTimeStep); +} + +idAFConstraint_ContactFriction::idAFConstraint_ContactFriction( + idAFConstraint_Contact* const contactConstraint) + : idAFConstraint("contactFriction", CONSTRAINT_FRICTION, + contactConstraint != nullptr ? contactConstraint->body1 : nullptr, + contactConstraint != nullptr ? contactConstraint->body2 : nullptr) + , cc(contactConstraint) { + InitSize(2); + fl.frameConstraint = 1; + fl.allowPrimary = 0; +} +bool idAFConstraint_ContactFriction::Add(idPhysics_AF* const af, float) { + if (cc == nullptr || body1 == nullptr) return false; + physics = af; + InitSize(2); + idVec3 tangent1; + idVec3 tangent2; + BasisForNormal(cc->contact.normal, tangent1, tangent2); + const idVec3 tangents[2] = {tangent1, tangent2}; + float friction = body1->contactFriction; + if (friction < 0.0f) friction = 0.0f; + friction *= af != nullptr ? GameLib_GetAFContactFrictionScale(af) : 1.0f; + for (int row = 0; row < 2; ++row) { + SetPointRow(J1, row, body1, cc->contact.point, + tangents[row], 1.0f); + if (body2 != nullptr) + SetPointRow(J2, row, body2, cc->contact.point, + tangents[row], -1.0f); + lo[row] = -friction; + hi[row] = friction; + boxIndex[row] = cc->firstIndex; + boxConstraint = cc; + } + GameLib_AddAFFrameConstraint(af, this); + return true; +} + +idAFConstraint_ConeLimit::idAFConstraint_ConeLimit() + : idAFConstraint("coneLimit", CONSTRAINT_CONELIMIT, nullptr, nullptr) + , coneAnchor(0.0f, 0.0f, 0.0f) + , coneAxis(0.0f, 0.0f, 1.0f) + , body1Axis(0.0f, 0.0f, 1.0f) + , cosAngle(1.0f) + , sinHalfAngle(0.0f) + , cosHalfAngle(1.0f) + , epsilon(0.001f) { + InitSize(1); + fl.allowPrimary = 0; +} +void idAFConstraint_ConeLimit::Setup(idAFBody* const firstBody, + idAFBody* const secondBody, const idVec3& anchor, + const idVec3& axis, const float angle, + const idVec3& firstBodyAxis) { + SetBody1(firstBody); + SetBody2(secondBody); + coneAnchor = anchor; + coneAxis = axis; + coneAxis.NormalizeFast(); + body1Axis = firstBodyAxis; + body1Axis.NormalizeFast(); + cosAngle = std::cos(angle * DEG2RAD); + sinHalfAngle = std::sin(angle * DEG2RAD * 0.5f); + cosHalfAngle = std::cos(angle * DEG2RAD * 0.5f); +} +bool idAFConstraint_ConeLimit::Add(idPhysics_AF* const af, + const float invTimeStep) { + const idVec3 cone = WorldAxis(body2, coneAxis); + const idVec3 body = WorldAxis(body1, body1Axis); + const float cosine = cone.Dot(body); + if (cosine >= cosAngle) return false; + physics = af; + InitSize(1); + idVec3 axis = cone.Cross(body); + axis.NormalizeFast(); + SetSpatialRow(J1, 0, kZeroVector, axis); + if (body2 != nullptr) SetSpatialRow(J2, 0, kZeroVector, -axis); + c1[0] = (std::acos(ClampUnit(cosine)) + - std::acos(ClampUnit(cosAngle))) * invTimeStep; + lo[0] = 0.0f; + hi[0] = 1.0e20f; + GameLib_AddAFFrameConstraint(af, this); + return true; +} +void idAFConstraint_ConeLimit::Translate(const idVec3& translation) { + if (body2 == nullptr) coneAnchor = coneAnchor + translation; +} +void idAFConstraint_ConeLimit::Rotate(const idRotation& rotation) { + if (body2 == nullptr) { + coneAnchor *= rotation; + coneAxis = rotation.ToMat3() * coneAxis; + } +} +void idAFConstraint_ConeLimit::DebugDraw() { idAFConstraint::DebugDraw(); } + +idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit() + : idAFConstraint("pyramidLimit", CONSTRAINT_PYRAMIDLIMIT, + nullptr, nullptr) + , pyramidAnchor(0.0f, 0.0f, 0.0f) + , pyramidBasis(1.0f) + , body1Axis(0.0f, 0.0f, 1.0f) + , cosAngle{1.0f, 1.0f} + , sinHalfAngle{0.0f, 0.0f} + , cosHalfAngle{1.0f, 1.0f} + , epsilon(0.001f) { + InitSize(1); + fl.allowPrimary = 0; +} +void idAFConstraint_PyramidLimit::Setup(idAFBody* const firstBody, + idAFBody* const secondBody, const idVec3& anchor, + const idVec3& axis, const idVec3& baseAxis, + const float angle1, const float angle2, + const idVec3& firstBodyAxis) { + SetBody1(firstBody); + SetBody2(secondBody); + pyramidAnchor = anchor; + pyramidBasis[0] = axis; + pyramidBasis[0].NormalizeFast(); + pyramidBasis[1] = baseAxis; + ProjectOntoPlane(pyramidBasis[1], pyramidBasis[0]); + pyramidBasis[1].NormalizeFast(); + pyramidBasis[2] = pyramidBasis[0].Cross(pyramidBasis[1]); + pyramidBasis[2].NormalizeFast(); + body1Axis = firstBodyAxis; + body1Axis.NormalizeFast(); + const float angles[2] = {angle1, angle2}; + for (int i = 0; i < 2; ++i) { + cosAngle[i] = std::cos(angles[i] * DEG2RAD); + sinHalfAngle[i] = std::sin(angles[i] * DEG2RAD * 0.5f); + cosHalfAngle[i] = std::cos(angles[i] * DEG2RAD * 0.5f); + } +} +bool idAFConstraint_PyramidLimit::Add(idPhysics_AF* const af, + const float invTimeStep) { + const idMat3 basis = BodyAxis(body2) * pyramidBasis; + const idVec3 body = WorldAxis(body1, body1Axis); + const float x = body.Dot(basis[0]); + const float y = body.Dot(basis[1]); + const float z = body.Dot(basis[2]); + const float limits[2] = {std::tan(std::acos(ClampUnit(cosAngle[0]))), + std::tan(std::acos(ClampUnit(cosAngle[1])))}; + const float excessY = std::fabs(y) - std::fabs(x) * limits[0]; + const float excessZ = std::fabs(z) - std::fabs(x) * limits[1]; + if (excessY <= 0.0f && excessZ <= 0.0f) return false; + physics = af; + InitSize(1); + idVec3 normal = excessY > excessZ ? basis[1] : basis[2]; + if (body.Dot(normal) < 0.0f) normal = -normal; + idVec3 axis = body.Cross(normal); + axis.NormalizeFast(); + SetSpatialRow(J1, 0, kZeroVector, axis); + if (body2 != nullptr) SetSpatialRow(J2, 0, kZeroVector, -axis); + c1[0] = (std::max)(excessY, excessZ) * invTimeStep; + lo[0] = 0.0f; + hi[0] = 1.0e20f; + GameLib_AddAFFrameConstraint(af, this); + return true; +} +void idAFConstraint_PyramidLimit::Translate(const idVec3& translation) { + if (body2 == nullptr) pyramidAnchor = pyramidAnchor + translation; +} +void idAFConstraint_PyramidLimit::Rotate(const idRotation& rotation) { + if (body2 == nullptr) { + pyramidAnchor *= rotation; + pyramidBasis *= rotation.ToMat3(); + } +} +void idAFConstraint_PyramidLimit::DebugDraw() { + idAFConstraint::DebugDraw(); +} + +idAFConstraint_Suspension::idAFConstraint_Suspension() + : idAFConstraint("suspension", CONSTRAINT_SUSPENSION, nullptr, nullptr) + , localOrigin(0.0f, 0.0f, 0.0f) + , suspensionUp(0.0f) + , suspensionDown(0.0f) + , suspensionCompress(0.0f) + , suspensionDamping(0.0f) + , friction_longitudinal(nullptr) + , friction_lateral(nullptr) + , frictionLatMultiplier(1.0f) + , frictionLongMultiplier(1.0f) + , clip(nullptr) + , wheelModel(nullptr) + , wheelRadius(0.0f) + , clipMask(0) + , cgLocation(0.0f, 0.0f, 0.0f) + , steerAngle(0.0f) + , motorEnabled(false) + , motorForce(0.0f) + , motorVelocity(0.0f) + , traceQuery{} + , traceFraction(1.0f) + , traceEndPos(0.0f, 0.0f, 0.0f) + , traceNormal(0.0f, 0.0f, 1.0f) + , tracePoint(0.0f, 0.0f, 0.0f) + , traceSurfaceFlags(0) + , traceEntityNum(ENTITYNUM_NONE) + , wheelVelocity(0.0f) + , wheelAngle(0.0f) + , lastWheelHeight(0.0f) + , lastMotorVelocity(0.0f) + , lastTorque(0.0f) + , lastGrip(0.0f) + , lastOnGround(false) + , limitLateralGrip(false) + , lateralSlip(0.0f) + , longitudinalSlip(0.0f) + , debugSpringForce(0.0f) + , debugv1(0.0f, 0.0f, 0.0f) + , debugv2(0.0f, 0.0f, 0.0f) + , debugv3(0.0f, 0.0f, 0.0f) + , debugf1(0.0f) + , debugf2(0.0f) + , debugf3(0.0f) + , debugf4(0.0f) { + InitSize(1); + fl.allowPrimary = 0; +} +void idAFConstraint_Suspension::Setup(const char* const suspensionName, + idAFBody* const body, const idVec3& origin, idClip* const ownerClip, + idClipModel* const model, const int mask, const float radius) { + name.Set(suspensionName != nullptr ? suspensionName : "suspension"); + SetBody1(body); + SetBody2(nullptr); + localOrigin = origin; + clip = ownerClip; + wheelModel = model; + clipMask = mask; + wheelRadius = radius; + traceEndPos = WorldPoint(body1, localOrigin); +} +void idAFConstraint_Suspension::SetSuspension(const float up, + const float down, const float compress, const float damping_) { + suspensionUp = up; + suspensionDown = down; + suspensionCompress = compress; + suspensionDamping = damping_; +} +int idAFConstraint_Suspension::GetContactEntity() const { + return lastOnGround ? traceEntityNum : ENTITYNUM_NONE; +} +void idAFConstraint_Suspension::Translate(const idVec3& translation) { + traceEndPos = traceEndPos + translation; +} +void idAFConstraint_Suspension::IssueCollisionQueries() { + traceFraction = 1.0f; + traceEntityNum = ENTITYNUM_NONE; + lastOnGround = false; + if (clip == nullptr || body1 == nullptr || wheelModel == nullptr) return; + const idVec3 start = WorldPoint(body1, localOrigin) + + BodyAxis(body1)[2] * suspensionUp; + const idVec3 end = WorldPoint(body1, localOrigin) + - BodyAxis(body1)[2] * suspensionDown; + trace_t result{}; + result.fraction = 1.0f; + result.endpos = end; + result.endAxis = BodyAxis(body1); + traceQuery = clip->Translation(&result, start, end, wheelModel, + BodyAxis(body1), clipMask, body1->clipModel != nullptr + ? body1->clipModel->GetEntityNumber() : ENTITYNUM_NONE, + false, "idAFConstraint_Suspension::IssueCollisionQueries"); + traceFraction = result.fraction; + traceEndPos = result.endpos; + traceNormal = result.c.normal; + tracePoint = result.c.point; + traceSurfaceFlags = result.c.surfaceFlags; + traceEntityNum = result.c.entityNum; + lastOnGround = traceFraction < 1.0f; +} +void idAFConstraint_Suspension::CancelCollisionQueries() { + traceQuery.index = 0; +} +void idAFConstraint_Suspension::DebugDraw() { + idAFConstraint::DebugDraw(); +} +void idAFConstraint_Suspension::Evaluate(const idPhysics_AF*, + const float invTimeStep) { + InitSize(1); + if (!lastOnGround || body1 == nullptr) return; + const idVec3 axis = BodyAxis(body1)[2]; + SetPointRow(J1, 0, body1, tracePoint, axis, 1.0f); + const float totalTravel = suspensionUp + suspensionDown; + const float compression = totalTravel * (1.0f - traceFraction); + const float pointVelocity = body1->GetPointVelocity(tracePoint).Dot(axis); + c1[0] = compression * suspensionCompress * invTimeStep + - pointVelocity * suspensionDamping; + lo[0] = 0.0f; + hi[0] = motorEnabled ? (std::max)(motorForce, 0.0f) : 1.0e20f; + debugSpringForce = c1[0]; + lastWheelHeight = compression; +} +void idAFConstraint_Suspension::Serialize(idSerializer* const serializer) { + if (serializer != nullptr) + GameLib_SerializeAFSuspension(serializer, *this); +} diff --git a/source/engine/gamelib/physics/afconstraint.h b/source/engine/gamelib/physics/afconstraint.h index 30bb0db..bfaf102 100644 --- a/source/engine/gamelib/physics/afconstraint.h +++ b/source/engine/gamelib/physics/afconstraint.h @@ -1,321 +1,422 @@ #pragma once -// Reconstructed C++ declarations from IDA Local Types and PDB/DIA metadata. -// Original PDB header: w:\tech5\engine\gamelib\physics\afconstraint.h -// Recovered logical types: 14 -// Signatures retain Xbox 360 ABI evidence and may still require manual review. +#include "gamelib/physics/afbody.h" +#include "gamelib/physics/clip.h" +#include "idlib/text/atomicstring.h" +#include +#include -// IDA Local Type ordinal 2471; PDB kind: enum. -enum declAFConstraintType_t : __int32 -{ - DECLAF_CONSTRAINT_INVALID = 0x0, - DECLAF_CONSTRAINT_FIXED = 0x1, - DECLAF_CONSTRAINT_BALLANDSOCKETJOINT = 0x2, - DECLAF_CONSTRAINT_UNIVERSALJOINT = 0x3, - DECLAF_CONSTRAINT_HINGE = 0x4, - DECLAF_CONSTRAINT_SLIDER = 0x5, - DECLAF_CONSTRAINT_SPRING = 0x6, +class idDeclTable; +class idPhysics_AF; +class idRotation; +class idSerializer; + +enum constraintType_t : int { + CONSTRAINT_INVALID = 0, + CONSTRAINT_FIXED = 1, + CONSTRAINT_BALLANDSOCKETJOINT = 2, + CONSTRAINT_UNIVERSALJOINT = 3, + CONSTRAINT_HINGE = 4, + CONSTRAINT_HINGESTEERING = 5, + CONSTRAINT_SLIDER = 6, + CONSTRAINT_CYLINDRICALJOINT = 7, + CONSTRAINT_PLANE = 9, + CONSTRAINT_SPRING = 10, + CONSTRAINT_CONTACT = 11, + CONSTRAINT_FRICTION = 12, + CONSTRAINT_CONELIMIT = 13, + CONSTRAINT_PYRAMIDLIMIT = 14, + CONSTRAINT_SUSPENSION = 15, + CONSTRAINT_WEEBLE = 16 }; -// IDA Local Type ordinal 2940; PDB kind: enum. -enum idDeclAF_Constraint::afConstraintLimit_t : __int32 -{ - LIMIT_NONE = 0xFFFFFFFF, - LIMIT_CONE = 0x0, - LIMIT_PYRAMID = 0x1, -}; - -// IDA Local Type ordinal 14601; PDB kind: class. -class idAFConstraint -{ +class alignas(16) idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 14624. - virtual ~idAFConstraint(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); + struct constraintFlags_s { + std::uint8_t reserved : 3; + std::uint8_t isZero : 1; + std::uint8_t isPrimary : 1; + std::uint8_t noCollision : 1; + std::uint8_t frameConstraint : 1; + std::uint8_t allowPrimary : 1; + }; - __declspec(align(16)) constraintType_t type; - idAtomicString name; - idAFBody *body1; - idAFBody *body2; - idPhysics_AF *physics; - idStaticSpatialVec lm; - idSpatialMat J1; - idSpatialMat J2; - idStaticSpatialVec c1; - idStaticSpatialVec c2; - idStaticSpatialVec lo; - idStaticSpatialVec hi; - idStaticSpatialVec e; - idAFConstraint *boxConstraint; - int boxIndex[6]; - float boxScale[6]; - idSpatialMat invI; - idSpatialMat J; - int firstIndex; - idAFConstraint::constraintFlags_s fl; + idAFConstraint(const char* name = nullptr, + constraintType_t type = CONSTRAINT_INVALID, + idAFBody* body1 = nullptr, idAFBody* body2 = nullptr); + virtual ~idAFConstraint(); + + static void* operator new(std::size_t size); + static void operator delete(void* pointer) noexcept; + + virtual void SetBody1(idAFBody*); + virtual void SetBody2(idAFBody*); + virtual void DebugDraw(); + virtual void Translate(const idVec3&); + virtual void Rotate(const idRotation&); + virtual void GetCenter(idVec3&) const; + virtual void IssueCollisionQueries(); + virtual void CancelCollisionQueries(); + virtual void Serialize(idSerializer*); + virtual void Evaluate(const idPhysics_AF*, float); + virtual void ApplyFriction(float); + + void InitSize(int size); + + alignas(16) constraintType_t type; + idAtomicString name; + idAFBody* body1; + idAFBody* body2; + idPhysics_AF* physics; + idStaticSpatialVec lm; + idSpatialMat J1; + idSpatialMat J2; + idStaticSpatialVec c1; + idStaticSpatialVec c2; + idStaticSpatialVec lo; + idStaticSpatialVec hi; + idStaticSpatialVec e; + idAFConstraint* boxConstraint; + int boxIndex[6]; + float boxScale[6]; + idSpatialMat invI; + idSpatialMat J; + int firstIndex; + constraintFlags_s fl; }; -// IDA Local Type ordinal 14615; PDB kind: class. -class __declspec(align(16)) idAFConstraint_Contact : public idAFConstraint -{ +class alignas(16) idAFConstraint_Fixed : public idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 33731. - virtual ~idAFConstraint_Contact(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); + idAFConstraint_Fixed(const char* name = nullptr, + idAFBody* body1 = nullptr, idAFBody* body2 = nullptr); + void SetBody1(idAFBody*) override; + void SetBody2(idAFBody*) override; + void Rotate(const idRotation&) override; + void GetCenter(idVec3&) const override; + void DebugDraw() override; + void Evaluate(const idPhysics_AF*, float) override; + void InitOffset(); - contactInfo_t contact; - float separation; - idAFConstraint_ContactFriction *fc; + idVec3 offset; + idMat3 relAxis; + bool shouldClamp; }; -// IDA Local Type ordinal 14625; PDB kind: struct. -struct idAFConstraint::constraintFlags_s -{ - unsigned __int8 : 3; - __int8 isZero : 1; - __int8 isPrimary : 1; - __int8 noCollision : 1; - __int8 frameConstraint : 1; - __int8 allowPrimary : 1; -}; +class idAFConstraint_ConeLimit; +class idAFConstraint_PyramidLimit; +class idAFConstraint_BallAndSocketJointFriction; -// IDA Local Type ordinal 14656; PDB kind: class. -class __declspec(align(4)) idDeclAF_Constraint -{ +class alignas(16) idAFConstraint_BallAndSocketJoint + : public idAFConstraint { public: - idAtomicString name; - idAtomicString body1; - idAtomicString body2; - declAFConstraintType_t type; - float friction; - float stretch; - float compress; - float damping; - float restLength; - float minLength; - float maxLength; - idAFVector anchor; - idAFVector anchor2; - idAFVector shaft[2]; - idAFVector axis; - idDeclAF_Constraint::afConstraintLimit_t limit; - idAFVector limitAxis; - float limitAngles[3]; - bool syncConstraint; + idAFConstraint_BallAndSocketJoint(const char* name = nullptr, + idAFBody* body1 = nullptr, idAFBody* body2 = nullptr); + ~idAFConstraint_BallAndSocketJoint() override; + void Translate(const idVec3&) override; + void Rotate(const idRotation&) override; + void GetCenter(idVec3&) const override; + void DebugDraw() override; + void Evaluate(const idPhysics_AF*, float) override; + void ApplyFriction(float) override; + void SetAnchor(const idVec3& worldPosition); + void SetWorldPosition(const idVec3& worldPosition); + float GetFriction() const; + void SetNoLimit(); + void SetConeLimit(const idVec3& coneAxis, float coneAngle, + const idVec3& body1Axis); + void SetPyramidLimit(const idVec3& pyramidAxis, + const idVec3& baseAxis, float angle1, float angle2, + const idVec3& body1Axis); + + idVec3 anchor1; + idVec3 anchor2; + float friction; + idAFConstraint_ConeLimit* coneLimit; + idAFConstraint_PyramidLimit* pyramidLimit; + idAFConstraint_BallAndSocketJointFriction* fc; }; -// IDA Local Type ordinal 21669; PDB kind: class. -class __declspec(align(16)) idAFConstraint_ConeLimit : public idAFConstraint -{ +class alignas(16) idAFConstraint_BallAndSocketJointFriction + : public idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 21670. - virtual ~idAFConstraint_ConeLimit(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); - - idVec3 coneAnchor; - idVec3 coneAxis; - idVec3 body1Axis; - float cosAngle; - float sinHalfAngle; - float cosHalfAngle; - float epsilon; + explicit idAFConstraint_BallAndSocketJointFriction( + idAFConstraint_BallAndSocketJoint* joint = nullptr); + bool Add(idPhysics_AF*, float invTimeStep); + idAFConstraint_BallAndSocketJoint* joint; }; -// IDA Local Type ordinal 21688; PDB kind: class. -class __declspec(align(16)) idAFConstraint_Fixed : public idAFConstraint -{ +class idAFConstraint_UniversalJointFriction; + +class alignas(16) idAFConstraint_UniversalJoint : public idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 21689. - virtual ~idAFConstraint_Fixed(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); + idAFConstraint_UniversalJoint(const char* name = nullptr, + idAFBody* body1 = nullptr, idAFBody* body2 = nullptr); + ~idAFConstraint_UniversalJoint() override; + void Translate(const idVec3&) override; + void Rotate(const idRotation&) override; + void GetCenter(idVec3&) const override; + void DebugDraw() override; + void Evaluate(const idPhysics_AF*, float) override; + void ApplyFriction(float) override; + void SetAnchor(const idVec3& worldPosition); + void SetShafts(const idVec3& cardanShaft1, + const idVec3& cardanShaft2); + float GetFriction() const; + void SetNoLimit(); + void SetConeLimit(const idVec3& coneAxis, float coneAngle, + const idVec3& body1Axis); + void SetPyramidLimit(const idVec3& pyramidAxis, + const idVec3& baseAxis, float angle1, float angle2, + const idVec3& body1Axis); - idVec3 offset; - idMat3 relAxis; - bool shouldClamp; + idVec3 anchor1; + idVec3 anchor2; + idVec3 shaft1; + idVec3 shaft2; + idVec3 axis1; + idVec3 axis2; + float friction; + idAFConstraint_ConeLimit* coneLimit; + idAFConstraint_PyramidLimit* pyramidLimit; + idAFConstraint_UniversalJointFriction* fc; }; -// IDA Local Type ordinal 21764; PDB kind: class. -class idAFConstraint_Line : public idAFConstraint -{ +class alignas(16) idAFConstraint_UniversalJointFriction + : public idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 21765. - virtual ~idAFConstraint_Line(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); - + explicit idAFConstraint_UniversalJointFriction( + idAFConstraint_UniversalJoint* joint = nullptr); + bool Add(idPhysics_AF*, float invTimeStep); + void Evaluate(const idPhysics_AF*, float) override; + idAFConstraint_UniversalJoint* joint; }; -// IDA Local Type ordinal 21844; PDB kind: class. -class __declspec(align(4)) idAFConstraint_Weeble : public idAFConstraint -{ +class idAFConstraint_HingeFriction; +class idAFConstraint_HingeSteering; + +class alignas(16) idAFConstraint_Hinge : public idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 21845. - virtual ~idAFConstraint_Weeble(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); + idAFConstraint_Hinge(const char* name = nullptr, + idAFBody* body1 = nullptr, idAFBody* body2 = nullptr); + ~idAFConstraint_Hinge() override; + void Translate(const idVec3&) override; + void Rotate(const idRotation&) override; + void GetCenter(idVec3&) const override; + void DebugDraw() override; + void Evaluate(const idPhysics_AF*, float) override; + void ApplyFriction(float) override; + void SetAnchor(const idVec3& worldPosition); + void SetAxis(const idVec3& axis); + float GetFriction() const; + float GetAngle() const; + void SetNoLimit(); + void SetLimit(float angle1, float angle2, float epsilon); - idVec3 up; - bool enabled; + idVec3 anchor1; + idVec3 anchor2; + idVec3 axis1; + idVec3 axis2; + idMat3 initialAxis; + float friction; + idAFConstraint_ConeLimit* coneLimit; + idAFConstraint_HingeSteering* steering; + idAFConstraint_HingeFriction* fc; }; -// IDA Local Type ordinal 21871; PDB kind: class. -class __declspec(align(16)) idAFConstraint_Hinge : public idAFConstraint -{ +class alignas(16) idAFConstraint_HingeFriction : public idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 21875. - virtual ~idAFConstraint_Hinge(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); - - idVec3 anchor1; - idVec3 anchor2; - idVec3 axis1; - idVec3 axis2; - idMat3 initialAxis; - float friction; - idAFConstraint_ConeLimit *coneLimit; - idAFConstraint_HingeSteering *steering; - idAFConstraint_HingeFriction *fc; + explicit idAFConstraint_HingeFriction( + idAFConstraint_Hinge* hinge = nullptr); + bool Add(idPhysics_AF*, float invTimeStep); + idAFConstraint_Hinge* hinge; }; -// IDA Local Type ordinal 21970; PDB kind: class. -class __declspec(align(16)) idAFConstraint_Plane : public idAFConstraint -{ +class idAFConstraint_HingeSteering : public idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 21971. - virtual ~idAFConstraint_Plane(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); - - idVec3 anchor1; - idVec3 anchor2; - idVec3 planeNormal; + explicit idAFConstraint_HingeSteering( + idAFConstraint_Hinge* hinge = nullptr); + bool Add(idPhysics_AF*, float invTimeStep); + idAFConstraint_Hinge* hinge; + float steerAngle; + float steerSpeed; + float epsilon; }; -// IDA Local Type ordinal 22048; PDB kind: class. -class idAFConstraint_Spring : public idAFConstraint -{ +class alignas(16) idAFConstraint_Slider : public idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 22049. - virtual ~idAFConstraint_Spring(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); - - idVec3 anchor1; - idVec3 anchor2; - float kstretch; - float kcompress; - float damping; - float restLength; - float minLength; - float maxLength; + idAFConstraint_Slider(const char* name = nullptr, + idAFBody* body1 = nullptr, idAFBody* body2 = nullptr); + void SetAxis(const idVec3&); + void GetCenter(idVec3&) const override; + void DebugDraw() override; + void Evaluate(const idPhysics_AF*, float) override; + idVec3 axis; + idVec3 offset; + idMat3 relAxis; }; -// IDA Local Type ordinal 22279; PDB kind: class. -class __declspec(align(8)) idAFConstraint_Slider : public idAFConstraint -{ +class idAFConstraint_Spring : public idAFConstraint { public: - // Recovered virtual interface; IDA vtable ordinal 22280. - virtual ~idAFConstraint_Slider(); - virtual void SetBody1(idAFBody *); - virtual void SetBody2(idAFBody *); - virtual void DebugDraw(); - virtual void Translate(const idVec3 *); - virtual void Rotate(const idRotation *); - virtual void GetCenter(idVec3 *); - virtual void IssueCollisionQueries(); - virtual void CancelCollisionQueries(); - virtual void Serialize(idSerializer *); - virtual void Evaluate(const idPhysics_AF *, float); - virtual void ApplyFriction(float); - - idVec3 axis; - idVec3 offset; - idMat3 relAxis; + idAFConstraint_Spring(const char* name = nullptr, + idAFBody* body1 = nullptr, idAFBody* body2 = nullptr); + void Translate(const idVec3&) override; + void Rotate(const idRotation&) override; + void GetCenter(idVec3&) const override; + void DebugDraw() override; + void Evaluate(const idPhysics_AF*, float) override; + void SetAnchor(const idVec3& anchor1, const idVec3& anchor2); + void SetSpring(float stretch, float compress, float damping, + float restLength); + void SetLimit(float minLength, float maxLength); + idVec3 anchor1; + idVec3 anchor2; + float kstretch; + float kcompress; + float damping; + float restLength; + float minLength; + float maxLength; }; + +class idAFConstraint_ContactFriction; + +class alignas(16) idAFConstraint_Contact : public idAFConstraint { +public: + idAFConstraint_Contact(); + ~idAFConstraint_Contact() override; + void GetCenter(idVec3&) const override; + void DebugDraw() override; + void ApplyFriction(float) override; + void Setup(idAFBody* body1, idAFBody* body2, + const contactInfo_t&, float separation, float invTimeStep); + contactInfo_t contact; + float separation; + idAFConstraint_ContactFriction* fc; +}; + +class alignas(16) idAFConstraint_ContactFriction + : public idAFConstraint { +public: + explicit idAFConstraint_ContactFriction( + idAFConstraint_Contact* contact = nullptr); + bool Add(idPhysics_AF*, float invTimeStep); + idAFConstraint_Contact* cc; +}; + +class alignas(16) idAFConstraint_ConeLimit : public idAFConstraint { +public: + idAFConstraint_ConeLimit(); + void Setup(idAFBody*, idAFBody*, const idVec3& coneAnchor, + const idVec3& coneAxis, float coneAngle, + const idVec3& body1Axis); + bool Add(idPhysics_AF*, float invTimeStep); + void Translate(const idVec3&) override; + void Rotate(const idRotation&) override; + void DebugDraw() override; + idVec3 coneAnchor; + idVec3 coneAxis; + idVec3 body1Axis; + float cosAngle; + float sinHalfAngle; + float cosHalfAngle; + float epsilon; +}; + +class alignas(16) idAFConstraint_PyramidLimit : public idAFConstraint { +public: + idAFConstraint_PyramidLimit(); + void Setup(idAFBody*, idAFBody*, const idVec3& pyramidAnchor, + const idVec3& pyramidAxis, const idVec3& baseAxis, + float angle1, float angle2, const idVec3& body1Axis); + bool Add(idPhysics_AF*, float invTimeStep); + void Translate(const idVec3&) override; + void Rotate(const idRotation&) override; + void DebugDraw() override; + idVec3 pyramidAnchor; + idMat3 pyramidBasis; + idVec3 body1Axis; + float cosAngle[2]; + float sinHalfAngle[2]; + float cosHalfAngle[2]; + float epsilon; +}; + +class alignas(16) idAFConstraint_Suspension : public idAFConstraint { +public: + idAFConstraint_Suspension(); + void Setup(const char* name, idAFBody*, const idVec3& localOrigin, + idClip*, idClipModel*, int clipMask, float radius); + void SetSuspension(float up, float down, float compress, + float damping); + int GetContactEntity() const; + void Translate(const idVec3&) override; + void IssueCollisionQueries() override; + void CancelCollisionQueries() override; + void DebugDraw() override; + void Evaluate(const idPhysics_AF*, float) override; + void Serialize(idSerializer*) override; + + idVec3 localOrigin; + float suspensionUp; + float suspensionDown; + float suspensionCompress; + float suspensionDamping; + const idDeclTable* friction_longitudinal; + const idDeclTable* friction_lateral; + float frictionLatMultiplier; + float frictionLongMultiplier; + idClip* clip; + idClipModel* wheelModel; + float wheelRadius; + int clipMask; + idVec3 cgLocation; + float steerAngle; + bool motorEnabled; + float motorForce; + float motorVelocity; + idClipQuery traceQuery; + float traceFraction; + idVec3 traceEndPos; + idVec3 traceNormal; + idVec3 tracePoint; + int traceSurfaceFlags; + int traceEntityNum; + float wheelVelocity; + float wheelAngle; + float lastWheelHeight; + float lastMotorVelocity; + float lastTorque; + float lastGrip; + bool lastOnGround; + bool limitLateralGrip; + float lateralSlip; + float longitudinalSlip; + float debugSpringForce; + idVec3 debugv1; + idVec3 debugv2; + idVec3 debugv3; + float debugf1; + float debugf2; + float debugf3; + float debugf4; +}; + +class idAFConstraint_Line : public idAFConstraint {}; +class idAFConstraint_CylindricalJoint : public idAFConstraint {}; + +class alignas(16) idAFConstraint_Plane : public idAFConstraint { +public: + idVec3 anchor1; + idVec3 anchor2; + idVec3 planeNormal; +}; + +class alignas(16) idAFConstraint_Weeble : public idAFConstraint { +public: + idVec3 up; + bool enabled; +}; + +static_assert(sizeof(idAFConstraint::constraintFlags_s) == 1, + "Recovered AF constraint flags ABI changed"); diff --git a/source/engine/gamelib/physics/physics_actor.h b/source/engine/gamelib/physics/physics_actor.h index adbdaa8..420151c 100644 --- a/source/engine/gamelib/physics/physics_actor.h +++ b/source/engine/gamelib/physics/physics_actor.h @@ -40,9 +40,10 @@ public: void LinkClip() override; bool EvaluateContacts() override; - void DisableClip(actorClipModel_t clipType); - void EnableClip(actorClipModel_t clipType); - void LinkClip(const idVec3& origin, const idMat3& axis); + virtual const idMat3* GetGravityAxis() { return &clipModelAxis; } + virtual void DisableClip(actorClipModel_t clipType); + virtual void EnableClip(actorClipModel_t clipType); + virtual void LinkClip(const idVec3& origin, const idMat3& axis); void SetClipModelAxis(); float GetMasterDeltaYaw() const; void RememberHistorySample(); diff --git a/source/engine/gamelib/physics/physics_af.cpp b/source/engine/gamelib/physics/physics_af.cpp new file mode 100644 index 0000000..91b7651 --- /dev/null +++ b/source/engine/gamelib/physics/physics_af.cpp @@ -0,0 +1,1281 @@ +#include "gamelib/physics/physics_af.h" + +#include "gamelib/physics/clipmodel.h" +#include "idlib/math/rotation.h" + +#include +#include +#include + +bool GameLib_GetMasterPhysicsTransform(idPhysicsCallbacks* callbacks, + idVec3& origin, idMat3& axis); +void GameLib_NotifyPhysicsActivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsDeactivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsCollision(idPhysicsCallbacks* callbacks, + int physicsId, const trace_t& collision, const idVec3& velocity); +void GameLib_SerializeAFPhysics(idSerializer* serializer, + idPhysics_AF& physics); + +namespace { + +constexpr int ENTITYNUM_NONE = 0x1FFF; +constexpr float RAD2DEG = 57.29577951308232f; +const idVec3 kZeroVector(0.0f, 0.0f, 0.0f); +const idMat3 kIdentityAxis(1.0f); +const idBounds kZeroBounds{{idVec3(0.0f, 0.0f, 0.0f), + idVec3(0.0f, 0.0f, 0.0f)}}; + +idAFBody* SelectBody(idList& bodies, const int id) { + if (bodies.Num() == 0) return nullptr; + if (id < 0) return bodies[0]; + return id < bodies.Num() ? bodies[id] : nullptr; +} + +const idAFBody* SelectBody(const idList& bodies, + const int id) { + if (bodies.Num() == 0) return nullptr; + if (id < 0) return bodies[0]; + return id < bodies.Num() ? bodies[id] : nullptr; +} + +idVec3 LinearVelocity(const idAFBody& body) { + return idVec3(body.current.spatialVelocity[0], + body.current.spatialVelocity[1], body.current.spatialVelocity[2]); +} + +idVec3 AngularVelocity(const idAFBody& body) { + return idVec3(body.current.spatialVelocity[3], + body.current.spatialVelocity[4], body.current.spatialVelocity[5]); +} + +void SetLinearVelocity(idAFBody& body, const idVec3& velocity) { + body.current.spatialVelocity[0] = velocity.x; + body.current.spatialVelocity[1] = velocity.y; + body.current.spatialVelocity[2] = velocity.z; +} + +void SetAngularVelocity(idAFBody& body, const idVec3& velocity) { + body.current.spatialVelocity[3] = velocity.x; + body.current.spatialVelocity[4] = velocity.y; + body.current.spatialVelocity[5] = velocity.z; +} + +void ZeroSpatial(idVec6& vector) { + for (int i = 0; i < 6; ++i) vector[i] = 0.0f; +} + +float RowVelocity(const idAFConstraint& constraint, const int row) { + float velocity = 0.0f; + if (constraint.body1 != nullptr) { + for (int i = 0; i < 6; ++i) + velocity += constraint.J1(row, i) + * constraint.body1->current.spatialVelocity[i]; + } + if (constraint.body2 != nullptr) { + for (int i = 0; i < 6; ++i) + velocity += constraint.J2(row, i) + * constraint.body2->current.spatialVelocity[i]; + } + return velocity; +} + +float RowEffectiveMass(const idAFConstraint& constraint, const int row) { + float inverseMass = 0.0f; + const idAFBody* bodyList[2] = {constraint.body1, constraint.body2}; + const idSpatialMat* jacobians[2] = {&constraint.J1, &constraint.J2}; + for (int bodyIndex = 0; bodyIndex < 2; ++bodyIndex) { + const idAFBody* const body = bodyList[bodyIndex]; + if (body == nullptr) continue; + const idSpatialMat& jacobian = *jacobians[bodyIndex]; + const idVec3 linear(jacobian(row, 0), jacobian(row, 1), + jacobian(row, 2)); + const idVec3 angular(jacobian(row, 3), jacobian(row, 4), + jacobian(row, 5)); + inverseMass += linear.LengthSqr() * body->invMass; + inverseMass += angular.Dot(body->inverseInertiaTensor * angular); + } + return (std::max)(inverseMass, 1.0e-6f); +} + +void ApplyConstraintImpulse(idAFBody* const body, + const idSpatialMat& jacobian, const int row, const float impulse) { + if (body == nullptr) return; + idVec3 linear(jacobian(row, 0), jacobian(row, 1), jacobian(row, 2)); + idVec3 angular(jacobian(row, 3), jacobian(row, 4), jacobian(row, 5)); + SetLinearVelocity(*body, LinearVelocity(*body) + + linear * (impulse * body->invMass)); + SetAngularVelocity(*body, AngularVelocity(*body) + + body->inverseInertiaTensor * angular * impulse); +} + +void SolveConstraintRows(idList& list, + const int iterations) { + for (int iteration = 0; iteration < iterations; ++iteration) { + for (int index = 0; index < list.Num(); ++index) { + idAFConstraint* const constraint = list[index]; + if (constraint == nullptr) continue; + const int rows = constraint->J1.GetNumRows(); + for (int row = 0; row < rows; ++row) { + float lambda = -(RowVelocity(*constraint, row) + + constraint->c1[row]) + / RowEffectiveMass(*constraint, row); + const float oldLambda = constraint->lm[row]; + const float next = (std::max)(constraint->lo[row], + (std::min)(constraint->hi[row], oldLambda + lambda)); + lambda = next - oldLambda; + constraint->lm[row] = next; + ApplyConstraintImpulse(constraint->body1, constraint->J1, + row, lambda); + ApplyConstraintImpulse(constraint->body2, constraint->J2, + row, lambda); + } + } + } +} + +bool IsDescendantOf(const idAFBody* body, const idAFBody* ancestor) { + for (const idAFBody* current = body; current != nullptr; + current = current->parent) { + if (current == ancestor) return true; + } + return false; +} + +void ExpandBounds(idBounds& destination, const idBounds& source) { + for (int axis = 0; axis < 3; ++axis) { + destination[0][axis] = (std::min)(destination[0][axis], + source[0][axis]); + destination[1][axis] = (std::max)(destination[1][axis], + source[1][axis]); + } +} + +} // namespace + +// Constraint code calls through these ownership-boundary functions so the +// circular AF/constraint dependency remains link-clean. +void GameLib_AddAFFrameConstraint(idPhysics_AF* const physics, + idAFConstraint* const constraint) { + if (physics != nullptr) physics->AddFrameConstraint(constraint); +} +float GameLib_GetAFJointFrictionScale(const idPhysics_AF* const physics) { + return physics != nullptr ? physics->GetJointFrictionScale() : 1.0f; +} +float GameLib_GetAFContactFrictionScale(const idPhysics_AF* const physics) { + return physics != nullptr ? physics->GetContactFrictionScale() : 1.0f; +} + +idPhysics_AF::idPhysics_AF() + : idPhysics_DynamicBase() + , trees(4) + , bodies(4) + , constraints(4) + , primaryConstraints(4) + , auxiliaryConstraints(4) + , frameConstraints(4) + , contactConstraints(4) + , contactBodies(4) + , noclipBodies() + , changedAF(true) + , linearFriction(0.005f) + , angularFriction(0.005f) + , contactFriction(0.8f) + , bouncyness(0.5f) + , totalMass(-1.0f) + , suspendVelocity(10.0f, 15.0f) + , suspendAcceleration(20.0f, 20.0f) + , noMoveTime(1.0f) + , noMoveTranslation(10.0f) + , noMoveRotation(10.0f) + , minMoveTime(-1.0f) + , maxMoveTime(-1.0f) + , impulseThreshold(1.0f) + , timeScale(1.0f) + , timeScaleRampStart(0.0f) + , timeScaleRampEnd(0.0f) + , jointFrictionScale(0.0f) + , jointFrictionDent(0.0f) + , jointFrictionDentStart(0.0f) + , jointFrictionDentEnd(0.0f) + , jointFrictionDentScale(0.0f) + , contactFrictionScale(0.0f) + , contactFrictionDent(0.0f) + , contactFrictionDentStart(0.0f) + , contactFrictionDentEnd(0.0f) + , contactFrictionDentScale(0.0f) + , errorReduction(idFader::FADE_LINEAR, 0.5f) + , errorReductionMax(idFader::FADE_LINEAR, 0.5f) + , lcpEpsilon(idFader::FADE_LINEAR, 1.0e-7f) + , limitErrorReduction(idFader::FADE_LINEAR, 0.3f) + , limitErrorReductionMax(idFader::FADE_LINEAR, 0.5f) + , limitLcpEpsilon(idFader::FADE_LINEAR, 1.0e-6f) + , contactErrorReduction(idFader::FADE_LINEAR, 0.7f) + , contactErrorReductionMax(idFader::FADE_LINEAR, 0.9f) + , contactLcpEpsilon(idFader::FADE_LINEAR, 1.0e-6f) + , universalErrorReduction(idFader::FADE_LINEAR, 0.5f) + , universalErrorReductionMax(idFader::FADE_LINEAR, 0.5f) + , universalTorsionLcpEpsilon(idFader::FADE_LINEAR, 1.0e-6f) + , passEntityNum(ENTITYNUM_NONE) + , selfCollision(true) + , comeToRest(true) + , linearTime(true) + , noImpact(false) + , worldConstraintsLocked(false) + , forcePushable(false) + , addContactConstraints(true) + , addGravity(true) + , masterBody(nullptr) + , current{} + , saved{} + , lastTimeStep(1.0f / 60.0f) + , endTime(0) + , timeStep(0.0f) + , lastImpulse(0.0f, 0.0f, 0.0f) + , lcp(nullptr) { + type = PHYSICS_AF; + current.atRest = false; + saved = current; + for (int i = 0; i < 8; ++i) { + noclipBodies.staticList[i].bodyId = -1; + noclipBodies.staticList[i].originalClipMask = 0; + noclipBodies.staticList[i].query.index = 0x100000000ULL; + } +} + +idPhysics_AF::~idPhysics_AF() { Shutdown(); } + +void idPhysics_AF::Shutdown() { + UnlinkClip(); + for (int i = 0; i < contactConstraints.Num(); ++i) + delete contactConstraints[i]; + contactConstraints.Clear(); + frameConstraints.Clear(); + for (int i = 0; i < constraints.Num(); ++i) delete constraints[i]; + constraints.Clear(); + primaryConstraints.Clear(); + auxiliaryConstraints.Clear(); + for (int i = 0; i < trees.Num(); ++i) delete trees[i]; + trees.Clear(); + for (int i = 0; i < bodies.Num(); ++i) delete bodies[i]; + bodies.Clear(); + contactBodies.Clear(); + noclipBodies.Clear(); + delete lcp; + lcp = nullptr; + masterBody = nullptr; +} + +void idPhysics_AF::SetSuspendTime(const float minimum, + const float maximum) { minMoveTime = minimum; maxMoveTime = maximum; } +void idPhysics_AF::SetSuspendTolerance(const float quietTime, + const float translation, const float rotation) { + noMoveTime = quietTime; + noMoveTranslation = translation; + noMoveRotation = rotation; +} +void idPhysics_AF::SetSuspendSpeed(const idVec2& velocity, + const idVec2& acceleration) { + suspendVelocity = velocity; + suspendAcceleration = acceleration; +} +void idPhysics_AF::SetTimeScaleRamp(const float start, const float end) { + timeScaleRampStart = start; + timeScaleRampEnd = end; +} +void idPhysics_AF::SetJointFrictionDent(const float dent, + const float start, const float end) { + jointFrictionDent = dent; + jointFrictionDentStart = start; + jointFrictionDentEnd = end; +} +void idPhysics_AF::SetContactFrictionDent(const float dent, + const float start, const float end) { + contactFrictionDent = dent; + contactFrictionDentStart = start; + contactFrictionDentEnd = end; +} +void idPhysics_AF::SetDefaultFriction(const float linear, + const float angular, const float contact) { + if (linear < 0.0f || linear > 1.0f || angular < 0.0f + || angular > 1.0f || contact < 0.0f || contact > 1.0f) return; + linearFriction = linear; + angularFriction = angular; + contactFriction = contact; +} + +float idPhysics_AF::GetJointFrictionScale() const { + float scale = jointFrictionScale > 0.0f ? jointFrictionScale : 1.0f; + if (current.activateTime >= jointFrictionDentStart + && current.activateTime <= jointFrictionDentEnd) + scale *= (std::max)(0.0f, 1.0f - jointFrictionDent); + return scale; +} +float idPhysics_AF::GetContactFrictionScale() const { + float scale = contactFrictionScale > 0.0f ? contactFrictionScale : 1.0f; + if (current.activateTime >= contactFrictionDentStart + && current.activateTime <= contactFrictionDentEnd) + scale *= (std::max)(0.0f, 1.0f - contactFrictionDent); + return scale; +} + +void idPhysics_AF::SetClipModel(idClipModel* const model, + const float density, const int id, const bool freeOld) { + idAFBody* const body = SelectBody(bodies, id); + if (body == nullptr) { + if (model != nullptr && freeOld) model->Delete(); + return; + } + if (!freeOld && body->clipModel != nullptr && body->clipModel != model) + body->clipModel->Unlink(); + body->SetClipModel(model); + if (density > 0.0f) body->SetDensity(density, kIdentityAxis); + changedAF = true; + Activate(); +} +idClipModel* idPhysics_AF::GetClipModel(const int id) { + idAFBody* const body = SelectBody(bodies, id); + return body != nullptr ? body->clipModel : nullptr; +} +int idPhysics_AF::GetNumClipModels() { return bodies.Num(); } +void idPhysics_AF::SetMass(const float newMass, const int id) { + idAFBody* const body = SelectBody(bodies, id); + if (body == nullptr || newMass <= 0.0f) return; + const float scale = newMass / (std::max)(body->mass, 1.0e-6f); + body->mass = newMass; + body->invMass = 1.0f / newMass; + for (int row = 0; row < 3; ++row) + for (int column = 0; column < 3; ++column) + body->inertiaTensor[row][column] *= scale; + body->inverseInertiaTensor = body->inertiaTensor; + body->inverseInertiaTensor.InverseSelf(); + totalMass = -1.0f; +} +float idPhysics_AF::GetMass(const int id) { + if (id >= 0) { + const idAFBody* const body = SelectBody(bodies, id); + return body != nullptr ? body->mass : 0.0f; + } + if (totalMass < 0.0f) { + totalMass = 0.0f; + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr) totalMass += bodies[i]->mass; + } + return totalMass; +} +void idPhysics_AF::SetContents(const int contents, const int id) { + if (id >= 0) { + idClipModel* const model = GetClipModel(id); + if (model != nullptr) model->SetContents(contents); + return; + } + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr && bodies[i]->clipModel != nullptr) + bodies[i]->clipModel->SetContents(contents); +} +int idPhysics_AF::GetContents(const int id) { + if (id >= 0) { + const idClipModel* const model = GetClipModel(id); + return model != nullptr ? model->GetContents() : 0; + } + int contents = 0; + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr && bodies[i]->clipModel != nullptr) + contents |= bodies[i]->clipModel->GetContents(); + return contents; +} +void idPhysics_AF::SetClipMask(const int mask, const int id) { + if (id >= 0) { + idAFBody* const body = SelectBody(bodies, id); + if (body != nullptr) { body->clipMask = mask; body->fl.clipMaskSet = 1; } + return; + } + clipMask = mask; + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr && !bodies[i]->fl.clipMaskSet) + bodies[i]->clipMask = mask; +} +int idPhysics_AF::GetClipMask(const int id) { + const idAFBody* const body = SelectBody(bodies, id); + return body != nullptr ? body->clipMask : clipMask; +} + +const idBounds* idPhysics_AF::GetBounds(const int id) { + if (id >= 0) { + const idClipModel* const model = GetClipModel(id); + return model != nullptr ? &model->GetBounds() : &kZeroBounds; + } + static thread_local idBounds bounds; + bool initialized = false; + for (int i = 0; i < bodies.Num(); ++i) { + const idClipModel* const model = bodies[i] != nullptr + ? bodies[i]->clipModel : nullptr; + if (model == nullptr) continue; + if (!initialized) { bounds = model->GetBounds(); initialized = true; } + else ExpandBounds(bounds, model->GetBounds()); + } + if (!initialized) bounds = kZeroBounds; + return &bounds; +} +const idBounds* idPhysics_AF::GetAbsBounds(const int id) { + if (id >= 0) { + const idClipModel* const model = GetClipModel(id); + return model != nullptr ? &model->GetAbsBounds() : &kZeroBounds; + } + static thread_local idBounds bounds; + bool initialized = false; + for (int i = 0; i < bodies.Num(); ++i) { + const idClipModel* const model = bodies[i] != nullptr + ? bodies[i]->clipModel : nullptr; + if (model == nullptr) continue; + if (!initialized) { bounds = model->GetAbsBounds(); initialized = true; } + else ExpandBounds(bounds, model->GetAbsBounds()); + } + if (!initialized) bounds = kZeroBounds; + return &bounds; +} + +void idPhysics_AF::SetOrigin(const idVec3* const origin, const int id) { + if (origin == nullptr) return; + idAFBody* const reference = SelectBody(bodies, id); + if (reference == nullptr) return; + const idVec3 delta = *origin - reference->current.worldOrigin; + Translate(&delta, id); +} +void idPhysics_AF::SetAxis(const idMat3* const axis, const int id) { + if (axis == nullptr) return; + idAFBody* const reference = SelectBody(bodies, id); + if (reference == nullptr) return; + const idMat3 rotationMatrix = *axis * reference->current.worldAxis.Transpose(); + const idVec3 pivot = reference->current.worldOrigin; + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr || (id >= 0 && i != id)) continue; + body->current.worldOrigin = pivot + + rotationMatrix * (body->current.worldOrigin - pivot); + body->current.worldAxis = rotationMatrix * body->current.worldAxis; + } + UpdateClipModels(); + Activate(); +} +void idPhysics_AF::Translate(const idVec3* const translation, const int id) { + if (translation == nullptr) return; + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr || (id >= 0 && i != id)) continue; + body->current.worldOrigin = body->current.worldOrigin + *translation; + } + if (id < 0) + for (int i = 0; i < constraints.Num(); ++i) + if (constraints[i] != nullptr) constraints[i]->Translate(*translation); + UpdateClipModels(); + Activate(); +} +void idPhysics_AF::Rotate(const idRotation* const rotation, const int id) { + if (rotation == nullptr) return; + const idMat3 matrix = rotation->ToMat3(); + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr || (id >= 0 && i != id)) continue; + body->current.worldOrigin = *rotation * body->current.worldOrigin; + body->current.worldAxis = matrix * body->current.worldAxis; + } + if (id < 0) + for (int i = 0; i < constraints.Num(); ++i) + if (constraints[i] != nullptr) constraints[i]->Rotate(*rotation); + UpdateClipModels(); + Activate(); +} +const idVec3* idPhysics_AF::GetOrigin(const int id) { + const idAFBody* const body = SelectBody(bodies, id); + return body != nullptr ? &body->current.worldOrigin : &kZeroVector; +} +const idMat3* idPhysics_AF::GetAxis(const int id) { + const idAFBody* const body = SelectBody(bodies, id); + return body != nullptr ? &body->current.worldAxis : &kIdentityAxis; +} +const idVec3* idPhysics_AF::GetLocalOrigin(const int id) { + return GetOrigin(id); +} +const idMat3* idPhysics_AF::GetLocalAxis(const int id) { return GetAxis(id); } + +void idPhysics_AF::SetLinearVelocity(const idVec3* const velocity, + const int id) { + if (velocity == nullptr) return; + if (id >= 0) { + idAFBody* const body = SelectBody(bodies, id); + if (body != nullptr) ::SetLinearVelocity(*body, *velocity); + } else { + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr) ::SetLinearVelocity(*bodies[i], *velocity); + } + Activate(); +} +void idPhysics_AF::SetAngularVelocity(const idVec3* const velocity, + const int id) { + if (velocity == nullptr) return; + if (id >= 0) { + idAFBody* const body = SelectBody(bodies, id); + if (body != nullptr) ::SetAngularVelocity(*body, *velocity); + } else { + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr) ::SetAngularVelocity(*bodies[i], *velocity); + } + Activate(); +} +idVec3* idPhysics_AF::GetLinearVelocity(idVec3* const result, + const int id) { + if (result == nullptr) return nullptr; + const idAFBody* const body = SelectBody(bodies, id); + *result = body != nullptr ? LinearVelocity(*body) : kZeroVector; + return result; +} +idVec3* idPhysics_AF::GetAngularVelocity(idVec3* const result, + const int id) { + if (result == nullptr) return nullptr; + const idAFBody* const body = SelectBody(bodies, id); + *result = body != nullptr ? AngularVelocity(*body) : kZeroVector; + return result; +} +void idPhysics_AF::SetWaterEntNum(int) {} +int idPhysics_AF::GetWaterEntNum() { return -1; } +void idPhysics_AF::SetWaterSurfaceWrldHeight(float) {} +float idPhysics_AF::GetWaterSurfaceWrldHeight() { return 0.0f; } + +void idPhysics_AF::GetImpactInfo(const int id, const idVec3* const point, + impactInfo_t* const info) { + if (info == nullptr) return; + info->Zero(); + const idAFBody* const body = SelectBody(bodies, id); + if (body == nullptr) return; + info->invMass = body->invMass; + info->invInertiaTensor = body->inverseInertiaTensor; + info->position = point != nullptr ? *point : body->current.worldOrigin; + info->velocity = body->GetPointVelocity(info->position); +} +void idPhysics_AF::ApplyImpulse(const int bodyId, const idVec3* const point, + const idVec3* const impulse) { + idAFBody* const body = SelectBody(bodies, bodyId); + if (body == nullptr || impulse == nullptr || noImpact) return; + const idVec3 applicationPoint = point != nullptr + ? *point : body->current.worldOrigin; + ::SetLinearVelocity(*body, LinearVelocity(*body) + + *impulse * body->invMass); + const idVec3 torque = (applicationPoint - body->current.worldOrigin) + .Cross(*impulse); + ::SetAngularVelocity(*body, AngularVelocity(*body) + + body->inverseInertiaTensor * torque); + lastImpulse = *impulse; + Activate(); +} +void idPhysics_AF::ApplyForce(const int bodyId, const idVec3* const point, + const idVec3* const force) { + idAFBody* const body = SelectBody(bodies, bodyId); + if (body == nullptr || force == nullptr) return; + body->AddForce(point != nullptr ? *point : body->current.worldOrigin, + *force); + Activate(); +} + +void idPhysics_AF::Activate() { + current.atRest = false; + current.noMoveTime = 0.0f; + current.activateTime = 0.0f; + if (callbacks != nullptr) + GameLib_NotifyPhysicsActivated(callbacks, GetPhysicsId()); +} +void idPhysics_AF::PutToRest() { + current.atRest = true; + for (int i = 0; i < bodies.Num(); ++i) { + if (bodies[i] == nullptr) continue; + ZeroSpatial(bodies[i]->current.spatialVelocity); + ZeroSpatial(bodies[i]->current.externalForce); + } +} +bool idPhysics_AF::IsAtRest() { return current.atRest; } +bool idPhysics_AF::IsPushable(int) { + return !noImpact && (masterBody == nullptr || forcePushable); +} + +void idPhysics_AF::SaveState() { + saved = current; + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr) bodies[i]->saved = bodies[i]->current; +} +void idPhysics_AF::RestoreState() { + current = saved; + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr) bodies[i]->current = bodies[i]->saved; + UpdateClipModels(); +} +void idPhysics_AF::UpdateTime(const int newEndTime) { endTime = newEndTime; } + +void idPhysics_AF::SetPushed(const int deltaTime) { + ZeroSpatial(current.pushVelocity); + if (deltaTime <= 0 || bodies.Num() == 0) return; + const float scale = 1000.0f / static_cast(deltaTime); + int count = 0; + for (int i = 0; i < bodies.Num(); ++i) { + const idAFBody* const body = bodies[i]; + if (body == nullptr) continue; + const idVec3 velocity = (body->current.worldOrigin + - body->saved.worldOrigin) * scale; + current.pushVelocity[0] += velocity.x; + current.pushVelocity[1] += velocity.y; + current.pushVelocity[2] += velocity.z; + ++count; + } + if (count > 0) + for (int i = 0; i < 3; ++i) current.pushVelocity[i] /= count; +} +idVec3* idPhysics_AF::GetPushedLinearVelocity(idVec3* const result, int) { + if (result != nullptr) result->Set(current.pushVelocity[0], + current.pushVelocity[1], current.pushVelocity[2]); + return result; +} +idVec3* idPhysics_AF::GetPushedAngularVelocity(idVec3* const result, int) { + if (result != nullptr) result->Set(current.pushVelocity[3], + current.pushVelocity[4], current.pushVelocity[5]); + return result; +} +void idPhysics_AF::SetAuxAngularVelocity(const idVec3& angular) { + current.auxVelocity[3] = angular.x; + current.auxVelocity[4] = angular.y; + current.auxVelocity[5] = angular.z; +} + +void idPhysics_AF::SetMaster(const bool enable, + const idVec3* const masterOrigin, const idMat3* const masterAxis, + bindFlags_t) { + if (enable && masterOrigin != nullptr && masterAxis != nullptr + && bodies.Num() > 0) { + masterBody = bodies[0]; + const idVec3 oldOrigin = masterBody->current.worldOrigin; + const idMat3 oldAxis = masterBody->current.worldAxis; + const idMat3 rotation = *masterAxis * oldAxis.Transpose(); + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr) continue; + body->current.worldOrigin = *masterOrigin + + rotation * (body->current.worldOrigin - oldOrigin); + body->current.worldAxis = rotation * body->current.worldAxis; + } + worldConstraintsLocked = true; + UpdateClipModels(); + } else { + masterBody = nullptr; + worldConstraintsLocked = false; + Activate(); + } +} +void idPhysics_AF::SetLocalOrigin(const idVec3* const origin, + const int id) { SetOrigin(origin, id); } +void idPhysics_AF::SetLocalAxis(const idMat3* const axis, + const int id) { SetAxis(axis, id); } +int idPhysics_AF::GetBlockingEntityNum() { + return contacts.Num() > 0 ? contacts[0].entityNum : ENTITYNUM_NONE; +} +int idPhysics_AF::GetLinearEndTime() { return 0; } +int idPhysics_AF::GetAngularEndTime() { return 0; } + +void idPhysics_AF::Serialize(idSerializer* const serializer) { + if (serializer != nullptr) GameLib_SerializeAFPhysics(serializer, *this); +} + +void idPhysics_AF::DisableClip() { + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr && bodies[i]->clipModel != nullptr) + bodies[i]->clipModel->Disable(); +} +void idPhysics_AF::EnableClip() { + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr && bodies[i]->clipModel != nullptr) + bodies[i]->clipModel->Enable(); +} +void idPhysics_AF::UnlinkClip() { + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr && bodies[i]->clipModel != nullptr) + bodies[i]->clipModel->Unlink(); +} +void idPhysics_AF::LinkClip() { UpdateClipModels(); } +void idPhysics_AF::UpdateClipModels() { + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr || body->clipModel == nullptr) continue; + body->clipModel->Link(GetEntityNumber(), GetEntityNumber(), i, + body->current.worldOrigin, body->current.worldAxis); + } +} + +void idPhysics_AF::ClipTranslation(trace_t* const result, + const idVec3* const translation, const idClipModel* const model) { + if (result == nullptr) return; + std::memset(result, 0, sizeof(*result)); + result->fraction = 1.0f; + if (translation == nullptr || clip == nullptr) return; + for (int i = 0; i < bodies.Num(); ++i) { + const idAFBody* const body = bodies[i]; + if (body == nullptr || body->clipModel == nullptr) continue; + trace_t local{}; + local.fraction = 1.0f; + if (model != nullptr) { + clip->TranslationModel(local, body->current.worldOrigin, + body->current.worldOrigin + *translation, body->clipModel, + body->current.worldAxis, body->clipMask, + model->GetOrigin(), model, model->GetAxis()); + } else { + clip->Translation(&local, body->current.worldOrigin, + body->current.worldOrigin + *translation, body->clipModel, + body->current.worldAxis, body->clipMask, passEntityNum, + false, "idPhysics_AF::ClipTranslation"); + } + if (local.fraction < result->fraction) *result = local; + } +} +void idPhysics_AF::ClipRotation(trace_t* const result, + const idRotation* const rotation, const idClipModel* const model) { + if (result == nullptr) return; + std::memset(result, 0, sizeof(*result)); + result->fraction = 1.0f; + if (rotation == nullptr || clip == nullptr) return; + for (int i = 0; i < bodies.Num(); ++i) { + const idAFBody* const body = bodies[i]; + if (body == nullptr || body->clipModel == nullptr) continue; + trace_t local{}; + local.fraction = 1.0f; + if (model != nullptr) { + clip->RotationModel(local, body->current.worldOrigin, + *rotation, body->clipModel, body->current.worldAxis, + body->clipMask, model->GetOrigin(), model, model->GetAxis()); + } else { + clip->Rotation(&local, body->current.worldOrigin, *rotation, + body->clipModel, body->current.worldAxis, body->clipMask, + passEntityNum, false, "idPhysics_AF::ClipRotation"); + } + if (local.fraction < result->fraction) *result = local; + } +} +int idPhysics_AF::ClipContents(const idClipModel* const model, + const int contentMask) { + if (clip == nullptr) return 0; + int contents = 0; + for (int i = 0; i < bodies.Num(); ++i) { + const idAFBody* const body = bodies[i]; + if (body == nullptr || body->clipModel == nullptr) continue; + trace_t local{}; + if (model != nullptr) { + clip->ContentsModel(local, body->current.worldOrigin, + body->clipModel, body->current.worldAxis, contentMask, + model->GetOrigin(), model, model->GetAxis()); + } else { + clip->Contents(&local, body->current.worldOrigin, + body->clipModel, body->current.worldAxis, contentMask, + passEntityNum, "idPhysics_AF::ClipContents"); + } + contents |= local.c.contentFlags; + } + return contents; +} + +int idPhysics_AF::AddBody(idAFBody* const body) { + if (body == nullptr) return -1; + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] == body) return i; + if (!body->fl.clipMaskSet) body->clipMask = clipMask; + if (body->linearFriction < 0.0f) body->linearFriction = linearFriction; + if (body->angularFriction < 0.0f) body->angularFriction = angularFriction; + if (body->contactFriction < 0.0f) body->contactFriction = contactFriction; + if (body->bouncyness < 0.0f) body->bouncyness = bouncyness; + bodies.Append(body); + totalMass = -1.0f; + changedAF = true; + return bodies.Num() - 1; +} +void idPhysics_AF::ForceBodyId(idAFBody* const body, const int newId) { + const int oldId = GetBodyId(body); + if (oldId < 0 || newId < 0 || newId >= bodies.Num() + || oldId == newId) return; + idAFBody* const displaced = bodies[newId]; + bodies[newId] = body; + bodies[oldId] = displaced; + changedAF = true; +} +int idPhysics_AF::GetBodyId(const idAFBody* const body) const { + for (int i = 0; i < bodies.Num(); ++i) if (bodies[i] == body) return i; + return -1; +} +idAFBody* idPhysics_AF::GetBody(const char* const bodyName) const { + if (bodyName == nullptr) return nullptr; + for (int i = 0; i < bodies.Num(); ++i) + if (bodies[i] != nullptr + && std::strcmp(bodies[i]->name.c_str(), bodyName) == 0) + return bodies[i]; + return nullptr; +} +idAFBody* idPhysics_AF::GetBody(const int id) const { + return const_cast(SelectBody(bodies, id)); +} +idAFConstraint* idPhysics_AF::GetConstraint( + const char* const constraintName) const { + if (constraintName == nullptr) return nullptr; + for (int i = 0; i < constraints.Num(); ++i) + if (constraints[i] != nullptr + && std::strcmp(constraints[i]->name.c_str(), + constraintName) == 0) + return constraints[i]; + return nullptr; +} +idAFConstraint* idPhysics_AF::GetConstraint(const int id) const { + return id >= 0 && id < constraints.Num() ? constraints[id] : nullptr; +} +void idPhysics_AF::AddConstraint(idAFConstraint* const constraint) { + if (constraint == nullptr) return; + for (int i = 0; i < constraints.Num(); ++i) + if (constraints[i] == constraint) return; + constraint->physics = this; + constraints.Append(constraint); + changedAF = true; +} +void idPhysics_AF::DeleteConstraint(const int id) { + if (id < 0 || id >= constraints.Num()) return; + idAFConstraint* const constraint = constraints[id]; + constraints.RemoveIndex(id); + primaryConstraints.Remove(constraint); + auxiliaryConstraints.Remove(constraint); + frameConstraints.Remove(constraint); + delete constraint; + changedAF = true; +} +void idPhysics_AF::DeleteConstraint(const char* const constraintName) { + idAFConstraint* const constraint = GetConstraint(constraintName); + if (constraint == nullptr) return; + DeleteConstraint(constraints.FindIndex(constraint)); +} +void idPhysics_AF::DeleteBody(const int id) { + if (id < 0 || id >= bodies.Num()) return; + idAFBody* const body = bodies[id]; + for (int i = constraints.Num() - 1; i >= 0; --i) + if (constraints[i] != nullptr + && (constraints[i]->body1 == body + || constraints[i]->body2 == body)) + DeleteConstraint(i); + bodies.RemoveIndex(id); + delete body; + totalMass = -1.0f; + changedAF = true; +} +void idPhysics_AF::AddFrameConstraint(idAFConstraint* const constraint) { + if (constraint == nullptr) return; + for (int i = 0; i < frameConstraints.Num(); ++i) + if (frameConstraints[i] == constraint) return; + constraint->physics = this; + constraint->fl.frameConstraint = 1; + frameConstraints.Append(constraint); +} + +int idPhysics_AF::AddNoclipBody(const int bodyId) { + idAFBody* const body = GetBody(bodyId); + if (body == nullptr || noclipBodies.Num() >= noclipBodies.Max()) return -1; + for (int i = 0; i < noclipBodies.Num(); ++i) + if (noclipBodies[i].bodyId == bodyId) return i; + noclipBodyInfo_t info{}; + info.bodyId = bodyId; + info.originalClipMask = body->clipMask; + info.query.index = 0; + noclipBodies.Append(info); + body->clipMask = 0; + return noclipBodies.Num() - 1; +} +void idPhysics_AF::TestNoclipBodies() { + if (clip == nullptr) return; + for (int i = noclipBodies.Num() - 1; i >= 0; --i) { + noclipBodyInfo_t& info = noclipBodies[i]; + idAFBody* const body = GetBody(info.bodyId); + if (body == nullptr || body->clipModel == nullptr) { + noclipBodies.RemoveIndex(i); + continue; + } + trace_t result{}; + info.query = clip->Contents(&result, body->current.worldOrigin, + body->clipModel, body->current.worldAxis, + info.originalClipMask, passEntityNum, + "idPhysics_AF::TestNoclipBodies"); + if (result.c.contentFlags == 0) { + body->clipMask = info.originalClipMask; + noclipBodies.RemoveIndex(i); + } + } +} + +void idPhysics_AF::ApplyFriction(const float step, float) { + const float jointScale = GetJointFrictionScale(); + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr) continue; + const float linear = body->linearFriction >= 0.0f + ? body->linearFriction : linearFriction; + const float angular = body->angularFriction >= 0.0f + ? body->angularFriction : angularFriction; + ::SetLinearVelocity(*body, LinearVelocity(*body) + * (std::max)(0.0f, 1.0f - linear * step)); + ::SetAngularVelocity(*body, AngularVelocity(*body) + * (std::max)(0.0f, 1.0f - angular * jointScale * step)); + } +} +void idPhysics_AF::AddGravity() { + if (!addGravity) return; + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr) continue; + body->current.externalForce[0] += gravityVector.x * body->mass; + body->current.externalForce[1] += gravityVector.y * body->mass; + body->current.externalForce[2] += gravityVector.z * body->mass; + } +} + +void idPhysics_AF::PrimaryFactor() { + for (int i = 0; i < trees.Num(); ++i) + if (trees[i] != nullptr) trees[i]->Factor(); +} +void idPhysics_AF::PrimaryForces(const float step) { + for (int i = 0; i < trees.Num(); ++i) + if (trees[i] != nullptr) trees[i]->CalculateForces(step); +} +void idPhysics_AF::AuxiliaryForces(float) { + SolveConstraintRows(auxiliaryConstraints, 8); + SolveConstraintRows(frameConstraints, 8); +} + +void idPhysics_AF::AddFrameConstraints() { + frameConstraints.Clear(); + const float inverseStep = timeStep > 0.0f ? 1.0f / timeStep : 0.0f; + for (int i = 0; i < constraints.Num(); ++i) + if (constraints[i] != nullptr) + constraints[i]->ApplyFriction(inverseStep); + for (int i = 0; i < contactConstraints.Num(); ++i) + if (contactConstraints[i] != nullptr) + contactConstraints[i]->ApplyFriction(inverseStep); +} +void idPhysics_AF::RemoveFrameConstraints() { frameConstraints.Clear(); } + +void idPhysics_AF::EvaluateConstraints(const float step) { + const float inverseStep = step > 0.0f ? 1.0f / step : 0.0f; + for (int i = 0; i < primaryConstraints.Num(); ++i) + if (primaryConstraints[i] != nullptr) + primaryConstraints[i]->Evaluate(this, inverseStep); + for (int i = 0; i < auxiliaryConstraints.Num(); ++i) + if (auxiliaryConstraints[i] != nullptr) + auxiliaryConstraints[i]->Evaluate(this, inverseStep); + for (int i = 0; i < contactConstraints.Num(); ++i) + if (contactConstraints[i] != nullptr) + contactConstraints[i]->Evaluate(this, inverseStep); + AddFrameConstraints(); + for (int i = 0; i < frameConstraints.Num(); ++i) + if (frameConstraints[i] != nullptr + && frameConstraints[i]->J1.GetNumRows() == 0) + frameConstraints[i]->Evaluate(this, inverseStep); + SolveConstraintRows(primaryConstraints, 8); + SolveConstraintRows(auxiliaryConstraints, 8); + // Contact rows are kept in their own authoritative list. + idList contactRows(4); + for (int i = 0; i < contactConstraints.Num(); ++i) + contactRows.Append(contactConstraints[i]); + SolveConstraintRows(contactRows, 8); + SolveConstraintRows(frameConstraints, 8); +} + +void idPhysics_AF::AddContacts(idAFBody* const body, + const contactsResult_t& result) { + if (body == nullptr) return; + const int count = (std::min)(12, result.numContacts); + for (int i = 0; i < count; ++i) { + const contactInfo_t& info = result.contacts[i]; + contacts.Append(info); + UpdateCollisionResidency(info); + bool bodyListed = false; + const int bodyId = GetBodyId(body); + for (int j = 0; j < contactBodies.Num(); ++j) + bodyListed |= contactBodies[j] == bodyId; + if (!bodyListed) contactBodies.Append(bodyId); + if (!addContactConstraints) continue; + idAFBody* otherBody = nullptr; + idPhysics* const otherPhysics = idPhysics::GetPhysicsForId( + info.physicsId); + if (otherPhysics == this) + otherBody = GetBody(info.bodyId); + idAFConstraint_Contact* const contact = + new idAFConstraint_Contact(); + contact->physics = this; + contact->Setup(body, otherBody, info, info.separation, + timeStep > 0.0f ? 1.0f / timeStep : 0.0f); + contactConstraints.Append(contact); + } +} +void idPhysics_AF::SetupContactConstraints(const float step) { + const float inverseStep = step > 0.0f ? 1.0f / step : 0.0f; + for (int i = 0; i < contactConstraints.Num(); ++i) { + idAFConstraint_Contact* const constraint = contactConstraints[i]; + if (constraint != nullptr) + constraint->Setup(constraint->body1, constraint->body2, + constraint->contact, constraint->separation, inverseStep); + } +} + +void idPhysics_AF::CollisionImpulse(idAFBody* const body, + const trace_t& collision, float) { + if (body == nullptr || collision.fraction >= 1.0f) return; + idVec3 velocity = body->GetPointVelocity(collision.c.point); + const float into = velocity.Dot(collision.c.normal); + if (into >= 0.0f) return; + const float bounce = body->bouncyness >= 0.0f + ? body->bouncyness : bouncyness; + const idVec3 impulse = collision.c.normal + * (-(1.0f + bounce) * into / (std::max)(body->invMass, 1.0e-6f)); + ApplyImpulse(GetBodyId(body), &collision.c.point, &impulse); + if (callbacks != nullptr) + GameLib_NotifyPhysicsCollision(callbacks, GetPhysicsId(), + collision, velocity); +} + +void idPhysics_AF::IssueCollisionQueries() { + for (int i = 0; i < constraints.Num(); ++i) + if (constraints[i] != nullptr) + constraints[i]->IssueCollisionQueries(); +} + +void idPhysics_AF::EvaluateBodies(const float step) { + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr) continue; + idVec3 linear = LinearVelocity(*body); + idVec3 angular = AngularVelocity(*body); + linear = linear + idVec3(body->current.externalForce[0], + body->current.externalForce[1], + body->current.externalForce[2]) * (body->invMass * step); + angular = angular + body->inverseInertiaTensor + * idVec3(body->current.externalForce[3], + body->current.externalForce[4], + body->current.externalForce[5]) * step; + ::SetLinearVelocity(*body, linear); + ::SetAngularVelocity(*body, angular); + const idVec3 start = body->current.worldOrigin; + const idVec3 end = start + linear * step; + idVec3 angularAxis = angular; + const float angularSpeed = angularAxis.NormalizeFast(); + idRotation rotation(start, angularSpeed > 0.0f ? angularAxis + : idVec3(0.0f, 0.0f, 1.0f), angularSpeed * step * RAD2DEG); + trace_t collision{}; + collision.fraction = 1.0f; + collision.endpos = end; + collision.endAxis = rotation.ToMat3() * body->current.worldAxis; + contactsResult_t contactResult{}; + if (clip != nullptr && body->clipModel != nullptr + && body->clipMask != 0) { + body->motionQuery = clip->MotionContacts(&collision, + &contactResult, start, end, rotation, 0.25f, + body->clipModel, body->current.worldAxis, body->clipMask, + passEntityNum, false, "idPhysics_AF::EvaluateBodies"); + } + body->current.worldOrigin = collision.endpos; + body->current.worldAxis = collision.endAxis; + if (contactResult.numContacts > 0) AddContacts(body, contactResult); + if (collision.fraction < 1.0f) + CollisionImpulse(body, collision, step); + ZeroSpatial(body->current.externalForce); + } + UpdateClipModels(); +} + +void idPhysics_AF::ResolveCollisions(float) { + AddContactPhysicsForContacts(); +} + +bool idPhysics_AF::EvaluateContacts() { + ClearContacts(); + for (int i = 0; i < contactConstraints.Num(); ++i) + delete contactConstraints[i]; + contactConstraints.Clear(); + contactBodies.Clear(); + if (clip == nullptr) return false; + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr || body->clipModel == nullptr) continue; + contactsResult_t result{}; + body->motionQuery = clip->Contacts(&result, + body->current.worldOrigin, gravityNormal, 0.25f, + body->clipModel, body->current.worldAxis, body->clipMask, + passEntityNum, "idPhysics_AF::EvaluateContacts"); + AddContacts(body, result); + } + AddContactPhysicsForContacts(); + return contacts.Num() > 0; +} + +bool idPhysics_AF::TestIfAtRest(const float step) { + float maximumLinear = 0.0f; + float maximumAngular = 0.0f; + float maximumAcceleration = 0.0f; + for (int i = 0; i < bodies.Num(); ++i) { + const idAFBody* const body = bodies[i]; + if (body == nullptr) continue; + maximumLinear = (std::max)(maximumLinear, LinearVelocity(*body).Length()); + maximumAngular = (std::max)(maximumAngular, AngularVelocity(*body).Length()); + maximumAcceleration = (std::max)(maximumAcceleration, + idVec3(body->acceleration[0], body->acceleration[1], + body->acceleration[2]).Length()); + } + const bool quiet = maximumLinear <= suspendVelocity.x + && maximumAngular <= suspendVelocity.y + && maximumAcceleration <= (std::max)(suspendAcceleration.x, + suspendAcceleration.y); + current.activateTime += step; + if (quiet) current.noMoveTime += step; + else current.noMoveTime = 0.0f; + const bool minimumElapsed = minMoveTime < 0.0f + || current.activateTime >= minMoveTime; + const bool maximumElapsed = maxMoveTime >= 0.0f + && current.activateTime >= maxMoveTime; + if (comeToRest && ((minimumElapsed && current.noMoveTime >= noMoveTime) + || maximumElapsed)) { + PutToRest(); + return true; + } + return false; +} + +void idPhysics_AF::Evolve(const float step) { + timeStep = step; + lastTimeStep = step; + ClearContacts(); + for (int i = 0; i < contactConstraints.Num(); ++i) + delete contactConstraints[i]; + contactConstraints.Clear(); + contactBodies.Clear(); + RemoveFrameConstraints(); + TestNoclipBodies(); + ApplyFriction(step, static_cast(endTime)); + AddGravity(); + IssueCollisionQueries(); + EvaluateConstraints(step); + EvaluateBodies(step); + ResolveCollisions(step); + EvaluateContacts(); + SetupContactConstraints(step); + TestIfAtRest(step); +} + +void idPhysics_AF::BuildTrees() { + for (int i = 0; i < trees.Num(); ++i) delete trees[i]; + trees.Clear(); + primaryConstraints.Clear(); + auxiliaryConstraints.Clear(); + for (int i = 0; i < bodies.Num(); ++i) { + if (bodies[i] == nullptr) continue; + bodies[i]->parent = nullptr; + bodies[i]->children.Clear(); + bodies[i]->primaryConstraint = nullptr; + bodies[i]->tree = nullptr; + } + for (int i = 0; i < constraints.Num(); ++i) { + idAFConstraint* const constraint = constraints[i]; + if (constraint == nullptr || !constraint->fl.allowPrimary + || constraint->body1 == nullptr + || constraint->body2 == nullptr + || constraint->body1->parent != nullptr + || IsDescendantOf(constraint->body2, constraint->body1)) { + if (constraint != nullptr) auxiliaryConstraints.Append(constraint); + continue; + } + constraint->fl.isPrimary = 1; + constraint->body1->parent = constraint->body2; + constraint->body1->primaryConstraint = constraint; + constraint->body2->children.Append(constraint->body1); + primaryConstraints.Append(constraint); + } + for (int i = 0; i < bodies.Num(); ++i) { + idAFBody* const body = bodies[i]; + if (body == nullptr || body->parent != nullptr) continue; + idAFTree* const tree = new idAFTree(); + tree->sortedBodies.Append(body); + tree->SortBodies(); + tree->SetMaxSubTreeAuxiliaryIndex(); + trees.Append(tree); + } + changedAF = false; +} + +void idPhysics_AF::DebugDraw() { + for (int i = 0; i < constraints.Num(); ++i) + if (constraints[i] != nullptr) constraints[i]->DebugDraw(); + const idVec4 color(0.2f, 0.8f, 1.0f, 1.0f); + for (int i = 0; i < trees.Num(); ++i) + if (trees[i] != nullptr) trees[i]->DebugDraw(color); +} + +float idPhysics_AF::GetLcpEpsilon(float) const { + return lcpEpsilon.GetValue(endTime); +} +float idPhysics_AF::GetErrorReduction(float) const { + return errorReduction.GetValue(endTime); +} +float idPhysics_AF::GetErrorReductionMax(float) const { + return errorReductionMax.GetValue(endTime); +} +float idPhysics_AF::GetLimitErrorReduction(float) const { + return limitErrorReduction.GetValue(endTime); +} +float idPhysics_AF::GetLimitErrorReductionMax(float) const { + return limitErrorReductionMax.GetValue(endTime); +} +float idPhysics_AF::GetLimitLcpEpsilon(float) const { + return limitLcpEpsilon.GetValue(endTime); +} +float idPhysics_AF::GetContactErrorReduction(float) const { + return contactErrorReduction.GetValue(endTime); +} +float idPhysics_AF::GetContactErrorReductionMax(float) const { + return contactErrorReductionMax.GetValue(endTime); +} +float idPhysics_AF::GetContactLcpEpsilon(float) const { + return contactLcpEpsilon.GetValue(endTime); +} +float idPhysics_AF::GetUniversalErrorReduction(float) const { + return universalErrorReduction.GetValue(endTime); +} +float idPhysics_AF::GetUniversalTorsionLcpEpsilon(float) const { + return universalTorsionLcpEpsilon.GetValue(endTime); +} + +bool idPhysics_AF::Evaluate(const int timeStepMSec, + const int newEndTime) { + endTime = newEndTime; + if (changedAF) BuildTrees(); + if (masterBody != nullptr && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) { + SetMaster(true, &masterOrigin, &masterAxis, + static_cast(0)); + } + } + if (current.atRest) return false; + float step = timeStepMSec * 0.001f * timeScale; + if (timeScaleRampEnd > timeScaleRampStart + && current.activateTime < timeScaleRampEnd) { + const float fraction = (std::max)(0.0f, (std::min)(1.0f, + (current.activateTime - timeScaleRampStart) + / (timeScaleRampEnd - timeScaleRampStart))); + step *= fraction; + } + if (step <= 0.0f) return false; + Evolve(step); + if (IsOutsideWorld()) { + PutToRest(); + if (callbacks != nullptr) + GameLib_NotifyPhysicsDeactivated(callbacks, GetPhysicsId()); + } + return true; +} + diff --git a/source/engine/gamelib/physics/physics_af.h b/source/engine/gamelib/physics/physics_af.h index 1c29b5b..e23265f 100644 --- a/source/engine/gamelib/physics/physics_af.h +++ b/source/engine/gamelib/physics/physics_af.h @@ -1,165 +1,223 @@ #pragma once -// Reconstructed C++ declarations from IDA Local Types and PDB/DIA metadata. -// Original PDB header: w:\tech5\engine\gamelib\physics\physics_af.h -// Recovered logical types: 2 -// Signatures retain Xbox 360 ABI evidence and may still require manual review. +#include "gamelib/physics/afconstraint.h" +#include "gamelib/physics/aftree.h" +#include "gamelib/physics/physics_dynamicbase.h" +#include "idlib/containers/staticlist.h" +#include "idlib/math/fader.h" +#include "idlib/math/lcp.h" +class idSerializer; -// IDA Local Type ordinal 14613; PDB kind: class. -class idPhysics_AF : public idPhysics_DynamicBase -{ +struct AFPState_t { + bool atRest; + float noMoveTime; + float activateTime; + idVec6 pushVelocity; + idVec6 auxVelocity; +}; + +class idPhysics_AF : public idPhysics_DynamicBase { public: - // Recovered virtual interface; IDA vtable ordinal 14623. - virtual ~idPhysics_AF(); - virtual void SetClipModel(idClipModel *, float, int, bool); - virtual idClipModel *GetClipModel(int); - virtual int GetNumClipModels(); - virtual void SetMass(float, int); - virtual float GetMass(int); - virtual void SetContents(int, int); - virtual int GetContents(int); - virtual void SetClipMask(int, int); - virtual int GetClipMask(int); - virtual const idBounds *GetBounds(int); - virtual const idBounds *GetAbsBounds(int); - virtual void SetOrigin(const idVec3 *, int); - virtual void SetAxis(const idMat3 *, int); - virtual void Translate(const idVec3 *, int); - virtual void Rotate(const idRotation *, int); - virtual const idVec3 *GetOrigin(int); - virtual const idMat3 *GetAxis(int); - virtual const idVec3 *GetLocalOrigin(int); - virtual const idMat3 *GetLocalAxis(int); - virtual void SetLinearVelocity(const idVec3 *, int); - virtual void SetAngularVelocity(const idVec3 *, int); - virtual idVec3 *GetLinearVelocity(idVec3 *result, int); - virtual idVec3 *GetAngularVelocity(idVec3 *result, int); - virtual void SetGravity(const idVec3 *); - virtual const idVec3 *GetGravity(); - virtual const idVec3 *GetGravityNormal(); - virtual void SetWaterLevel(float, int); - virtual float GetWaterLevel(int); - virtual void SetWaterViscosity(float, int); - virtual float GetWaterViscosity(int); - virtual void SetWaterEntNum(int); - virtual int GetWaterEntNum(); - virtual void SetWaterSurfaceWrldHeight(float); - virtual float GetWaterSurfaceWrldHeight(); - virtual void GetImpactInfo(const int, const idVec3 *, impactInfo_t *); - virtual void ApplyImpulse(const int, const idVec3 *, const idVec3 *); - virtual void ApplyForce(const int, const idVec3 *, const idVec3 *); - virtual void Activate(); - virtual void PutToRest(); - virtual bool IsAtRest(); - virtual bool IsPushable(int); - virtual void SaveState(); - virtual void RestoreState(); - virtual bool Evaluate(int, int); - virtual void UpdateTime(int); - virtual void ClipTranslation(trace_t *, const idVec3 *, const idClipModel *); - virtual void ClipRotation(trace_t *, const idRotation *, const idClipModel *); - virtual int ClipContents(const idClipModel *, int); - virtual void DisableClip(); - virtual void EnableClip(); - virtual void UnlinkClip(); - virtual void LinkClip(); - virtual bool EvaluateContacts(); - virtual int GetNumContacts(); - virtual const contactInfo_t *GetContact(int); - virtual void ClearContacts(); - virtual void AddContactPhysics(idPhysics *); - virtual void RemoveContactPhysics(idPhysics *); - virtual int GetNumContactPhysics(); - virtual idPhysics *GetContactPhysics(int); - virtual void ActivateContactPhysics(); - virtual bool HasGroundContacts(); - virtual bool IsGroundEntity(int); - virtual bool IsGroundClipModel(int, int); - virtual void SetPushed(int); - virtual idVec3 *GetPushedLinearVelocity(idVec3 *result, const int); - virtual idVec3 *GetPushedAngularVelocity(idVec3 *result, const int); - virtual void SetMaster(bool, const idVec3 *, const idMat3 *, const bindFlags_t); - virtual void SetLocalOrigin(const idVec3 *, int); - virtual void SetLocalAxis(const idMat3 *, int); - virtual int GetBlockingEntityNum(); - virtual int GetLinearEndTime(); - virtual int GetAngularEndTime(); - virtual bool IsInNonResidentCollisionArea(bool); - virtual bool IsOutsideWorld(); - virtual void Serialize(idSerializer *); + struct noclipBodyInfo_t { + int bodyId; + int originalClipMask; + idClipQuery query; + }; - idList trees; - idList bodies; - idList constraints; - idList primaryConstraints; - idList auxiliaryConstraints; - idList frameConstraints; - idList contactConstraints; - idList contactBodies; - idStaticList noclipBodies; - bool changedAF; - float linearFriction; - float angularFriction; - float contactFriction; - float bouncyness; - float totalMass; - idVec2 suspendVelocity; - idVec2 suspendAcceleration; - float noMoveTime; - float noMoveTranslation; - float noMoveRotation; - float minMoveTime; - float maxMoveTime; - float impulseThreshold; - float timeScale; - float timeScaleRampStart; - float timeScaleRampEnd; - float jointFrictionScale; - float jointFrictionDent; - float jointFrictionDentStart; - float jointFrictionDentEnd; - float jointFrictionDentScale; - float contactFrictionScale; - float contactFrictionDent; - float contactFrictionDentStart; - float contactFrictionDentEnd; - float contactFrictionDentScale; - idFader errorReduction; - idFader errorReductionMax; - idFader lcpEpsilon; - idFader limitErrorReduction; - idFader limitErrorReductionMax; - idFader limitLcpEpsilon; - idFader contactErrorReduction; - idFader contactErrorReductionMax; - idFader contactLcpEpsilon; - idFader universalErrorReduction; - idFader universalErrorReductionMax; - idFader universalTorsionLcpEpsilon; - int passEntityNum; - bool selfCollision; - bool comeToRest; - bool linearTime; - bool noImpact; - bool worldConstraintsLocked; - bool forcePushable; - bool addContactConstraints; - bool addGravity; - idAFBody *masterBody; - AFPState_t current; - AFPState_t saved; - float lastTimeStep; - int endTime; - float timeStep; - idVec3 lastImpulse; - idLCP *lcp; + idPhysics_AF(); + ~idPhysics_AF() override; + + void SetClipModel(idClipModel*, float, int, bool) override; + idClipModel* GetClipModel(int) override; + int GetNumClipModels() override; + void SetMass(float, int) override; + float GetMass(int) override; + void SetContents(int, int) override; + int GetContents(int) override; + void SetClipMask(int, int) override; + int GetClipMask(int) override; + const idBounds* GetBounds(int) override; + const idBounds* GetAbsBounds(int) override; + void SetOrigin(const idVec3*, int) override; + void SetAxis(const idMat3*, int) override; + void Translate(const idVec3*, int) override; + void Rotate(const idRotation*, int) override; + const idVec3* GetOrigin(int) override; + const idMat3* GetAxis(int) override; + const idVec3* GetLocalOrigin(int) override; + const idMat3* GetLocalAxis(int) override; + void SetLinearVelocity(const idVec3*, int) override; + void SetAngularVelocity(const idVec3*, int) override; + idVec3* GetLinearVelocity(idVec3*, int) override; + idVec3* GetAngularVelocity(idVec3*, int) override; + void SetWaterEntNum(int) override; + int GetWaterEntNum() override; + void SetWaterSurfaceWrldHeight(float) override; + float GetWaterSurfaceWrldHeight() override; + void GetImpactInfo(int, const idVec3*, impactInfo_t*) override; + void ApplyImpulse(int, const idVec3*, const idVec3*) override; + void ApplyForce(int, const idVec3*, const idVec3*) override; + void Activate() override; + void PutToRest() override; + bool IsAtRest() override; + bool IsPushable(int) override; + void SaveState() override; + void RestoreState() override; + bool Evaluate(int, int) override; + void UpdateTime(int) override; + void ClipTranslation(trace_t*, const idVec3*, + const idClipModel*) override; + void ClipRotation(trace_t*, const idRotation*, + const idClipModel*) override; + int ClipContents(const idClipModel*, int) override; + void DisableClip() override; + void EnableClip() override; + void UnlinkClip() override; + void LinkClip() override; + bool EvaluateContacts() override; + void SetPushed(int) override; + idVec3* GetPushedLinearVelocity(idVec3*, int) override; + idVec3* GetPushedAngularVelocity(idVec3*, int) override; + void SetMaster(bool, const idVec3*, const idMat3*, + bindFlags_t) override; + void SetLocalOrigin(const idVec3*, int) override; + void SetLocalAxis(const idMat3*, int) override; + int GetBlockingEntityNum() override; + int GetLinearEndTime() override; + int GetAngularEndTime() override; + + void Serialize(idSerializer*); + void Shutdown(); + void SetSuspendTime(float minTime, float maxTime); + void SetSuspendTolerance(float translation, float rotation, + float noMoveTime_); + void SetSuspendSpeed(const idVec2& velocity, + const idVec2& acceleration); + void SetTimeScaleRamp(float start, float end); + void SetJointFrictionDent(float dent, float start, float end); + void SetContactFrictionDent(float dent, float start, float end); + void SetDefaultFriction(float linear, float angular, float contact); + float GetJointFrictionScale() const; + float GetContactFrictionScale() const; + void SetAuxAngularVelocity(const idVec3&); + void ForceBodyId(idAFBody*, int newId); + int GetBodyId(const idAFBody*) const; + idAFBody* GetBody(const char* name) const; + idAFBody* GetBody(int id) const; + idAFConstraint* GetConstraint(const char* name) const; + idAFConstraint* GetConstraint(int id) const; + int AddBody(idAFBody*); + void DeleteBody(int); + void AddConstraint(idAFConstraint*); + void DeleteConstraint(int); + void DeleteConstraint(const char* name); + void AddFrameConstraint(idAFConstraint*); + int AddNoclipBody(int bodyId); + + void ApplyFriction(float timeStep, float endTimeMSec); + void PrimaryFactor(); + void PrimaryForces(float timeStep); + void AuxiliaryForces(float timeStep); + void AddGravity(); + void UpdateClipModels(); + bool TestIfAtRest(float timeStep); + void EvaluateBodies(float timeStep); + void EvaluateConstraints(float timeStep); + void CollisionImpulse(idAFBody*, const trace_t&, float timeStep); + void IssueCollisionQueries(); + void ResolveCollisions(float timeStep); + void AddFrameConstraints(); + void RemoveFrameConstraints(); + void SetupContactConstraints(float timeStep); + void AddContacts(idAFBody*, const contactsResult_t&); + void TestNoclipBodies(); + void Evolve(float timeStep); + void BuildTrees(); + void DebugDraw(); + + float GetLcpEpsilon(float timeStep) const; + float GetErrorReduction(float timeStep) const; + float GetErrorReductionMax(float timeStep) const; + float GetLimitErrorReduction(float timeStep) const; + float GetLimitErrorReductionMax(float timeStep) const; + float GetLimitLcpEpsilon(float timeStep) const; + float GetContactErrorReduction(float timeStep) const; + float GetContactErrorReductionMax(float timeStep) const; + float GetContactLcpEpsilon(float timeStep) const; + float GetUniversalErrorReduction(float timeStep) const; + float GetUniversalTorsionLcpEpsilon(float timeStep) const; + + idList trees; + idList bodies; + idList constraints; + idList primaryConstraints; + idList auxiliaryConstraints; + idList frameConstraints; + idList contactConstraints; + idList contactBodies; + idStaticList noclipBodies; + bool changedAF; + float linearFriction; + float angularFriction; + float contactFriction; + float bouncyness; + float totalMass; + idVec2 suspendVelocity; + idVec2 suspendAcceleration; + float noMoveTime; + float noMoveTranslation; + float noMoveRotation; + float minMoveTime; + float maxMoveTime; + float impulseThreshold; + float timeScale; + float timeScaleRampStart; + float timeScaleRampEnd; + float jointFrictionScale; + float jointFrictionDent; + float jointFrictionDentStart; + float jointFrictionDentEnd; + float jointFrictionDentScale; + float contactFrictionScale; + float contactFrictionDent; + float contactFrictionDentStart; + float contactFrictionDentEnd; + float contactFrictionDentScale; + idFader errorReduction; + idFader errorReductionMax; + idFader lcpEpsilon; + idFader limitErrorReduction; + idFader limitErrorReductionMax; + idFader limitLcpEpsilon; + idFader contactErrorReduction; + idFader contactErrorReductionMax; + idFader contactLcpEpsilon; + idFader universalErrorReduction; + idFader universalErrorReductionMax; + idFader universalTorsionLcpEpsilon; + int passEntityNum; + bool selfCollision; + bool comeToRest; + bool linearTime; + bool noImpact; + bool worldConstraintsLocked; + bool forcePushable; + bool addContactConstraints; + bool addGravity; + idAFBody* masterBody; + AFPState_t current; + AFPState_t saved; + float lastTimeStep; + int endTime; + float timeStep; + idVec3 lastImpulse; + idLCP* lcp; }; -// IDA Local Type ordinal 14619; PDB kind: struct. -struct idPhysics_AF::noclipBodyInfo_t -{ - int bodyId; - int originalClipMask; - idClipQuery query; -}; +static_assert(sizeof(AFPState_t) == 60, + "Recovered articulated-figure physics state ABI changed"); +#if defined(_WIN32) && !defined(_WIN64) +static_assert(sizeof(idPhysics_AF) == 912, + "Recovered idPhysics_AF ABI changed"); +#endif diff --git a/source/engine/gamelib/physics/physics_ai.cpp b/source/engine/gamelib/physics/physics_ai.cpp new file mode 100644 index 0000000..6da5b2e --- /dev/null +++ b/source/engine/gamelib/physics/physics_ai.cpp @@ -0,0 +1,449 @@ +#include "gamelib/physics/physics_ai.h" + +#include "gamelib/physics/clipmodel.h" +#include "idlib/geometry/tracemodel.h" +#include "idlib/math/rotation.h" + +#include +#include +#include + +bool GameLib_GetMasterPhysicsTransform(idPhysicsCallbacks* callbacks, + idVec3& origin, idMat3& axis); +void GameLib_NotifyPhysicsActivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsDeactivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsCollision(idPhysicsCallbacks* callbacks, + int physicsId, const trace_t& collision, const idVec3& velocity); + +namespace { + +const idVec3 kAIZeroVector(0.0f, 0.0f, 0.0f); +const idMat3 kAIIdentityAxis(1.0f); + +float AxisYaw(const idMat3& axis) { + return std::atan2(axis[0].y, axis[0].x) * 57.29577951308232f; +} + +void ProjectOntoPlane(idVec3& vector, const idVec3& normal, + const float overBounce = 1.0f) { + if (normal.LengthSqr() <= 1.0e-12f) return; + vector = vector - normal * (vector.Dot(normal) * overBounce); +} + +} // namespace + +idPhysics_AI::idPhysics_AI() + : idPhysics_Actor() + , current{} + , saved{} + , stepMoveQuery{0} + , contactsQuery{0} + , lastCollisionNormal(0.0f, 0.0f, 0.0f) + , lastCollisionPoint(0.0f, 0.0f, 0.0f) + , stuckCollisionNormal(0.0f, 0.0f, 0.0f) + , stuckCollisionPoint(0.0f, 0.0f, 0.0f) + , clipModel_standing(nullptr) + , clipModel_crouched(nullptr) + , maxStepHeight(18.0f) + , minFloorCosine(0.70710677f) + , maxDropVelocity(-500.0f) + , heightCrouched(48.0f) + , movementType(AI_MOVE_WALKING) + , noImpact(false) + , masterControlledVelocity(false) + , swimmer(false) + , crouched(false) + , canSetCrouchedPhysics(false) { + type = PHYSICS_AI; + current.atRest = -1; + saved = current; +} + +idPhysics_AI::~idPhysics_AI() { + UnlinkClip(); + if (clipModel_crouched != nullptr + && clipModel_crouched != clipModel_standing) + clipModel_crouched->Delete(); + if (clipModel_standing != nullptr) clipModel_standing->Delete(); + clipModel_standing = nullptr; + clipModel_crouched = nullptr; + clipModels[0] = nullptr; +} + +void idPhysics_AI::SetClipModel(idClipModel* const model, + const float density, const int id, const bool freeOld) { + if (id != 0) { + idPhysics_Actor::SetClipModel(model, density, id, freeOld); + return; + } + if (freeOld) { + if (clipModel_crouched != nullptr + && clipModel_crouched != clipModel_standing) + clipModel_crouched->Delete(); + if (clipModel_standing != nullptr && clipModel_standing != model) + clipModel_standing->Delete(); + } + clipModel_standing = model; + clipModel_crouched = nullptr; + clipModels[0] = nullptr; + if (model != nullptr && clip != nullptr) { + idBounds crouchedBounds = model->GetBounds(); + crouchedBounds[1].z = heightCrouched; + if (crouchedBounds[1].z < crouchedBounds[0].z) + crouchedBounds[1].z = crouchedBounds[0].z; + idTraceModel crouchedTrace{}; + crouchedTrace.SetupBox(crouchedBounds); + clipModel_crouched = new idClipModel(clip, &crouchedTrace, -1, + nullptr); + clipModel_crouched->SetContents(model->GetContents()); + } + idPhysics_Actor::SetClipModel(model, density, 0, false); + current.worldOrigin = model != nullptr ? model->GetOrigin() + : kAIZeroVector; + current.localOrigin = current.worldOrigin; + LinkClip(current.worldOrigin, model != nullptr ? model->GetAxis() + : kAIIdentityAxis); +} + +void idPhysics_AI::SetContents(const int contents, const int id) { + if (id <= 0) { + if (clipModel_standing != nullptr) + clipModel_standing->SetContents(contents); + if (clipModel_crouched != nullptr) + clipModel_crouched->SetContents(contents); + } else { + idPhysics_Actor::SetContents(contents, id); + } +} + +void idPhysics_AI::SetOrigin(const idVec3* const newOrigin, int) { + if (newOrigin == nullptr) return; + stepMoveQuery.index = 0; + const idMat3 axis = clipModels[0] != nullptr + ? clipModels[0]->GetAxis() : clipModelAxis; + LinkClip(*newOrigin, axis); + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current.localOrigin = masterAxis.Transpose() + * (*newOrigin - masterOrigin); + } else { + current.localOrigin = *newOrigin; + } + Activate(); +} + +void idPhysics_AI::SetAxis(const idMat3* const newAxis, int) { + if (newAxis == nullptr) return; + stepMoveQuery.index = 0; + LinkClip(current.worldOrigin, *newAxis); + Activate(); +} + +void idPhysics_AI::Translate(const idVec3* const translation, int) { + if (translation == nullptr) return; + ResolveCollisions(); + SetOrigin(&(current.worldOrigin = current.worldOrigin + *translation), 0); +} + +void idPhysics_AI::Rotate(const idRotation* const rotation, int) { + if (rotation == nullptr) return; + ResolveCollisions(); + current.worldOrigin = *rotation * current.worldOrigin; + const idMat3 axis = (clipModels[0] != nullptr + ? clipModels[0]->GetAxis() : clipModelAxis) * rotation->ToMat3(); + LinkClip(current.worldOrigin, axis); + current.localOrigin = *rotation * current.localOrigin; + Activate(); +} + +const idVec3* idPhysics_AI::GetLocalOrigin(int) { + return ¤t.localOrigin; +} +const idMat3* idPhysics_AI::GetLocalAxis(int) { + return clipModels[0] != nullptr ? &clipModels[0]->GetAxis() + : &kAIIdentityAxis; +} + +void idPhysics_AI::SetLinearVelocity(const idVec3* const velocity, int) { + if (velocity == nullptr) return; + current.velocity = *velocity; + Activate(); +} +void idPhysics_AI::SetAngularVelocity(const idVec3*, int) {} +idVec3* idPhysics_AI::GetLinearVelocity(idVec3* const result, int) { + if (result != nullptr) *result = current.velocity; + return result; +} +idVec3* idPhysics_AI::GetAngularVelocity(idVec3* const result, int) { + if (result != nullptr) result->Zero(); + return result; +} +void idPhysics_AI::SetWaterEntNum(int) {} +int idPhysics_AI::GetWaterEntNum() { return -1; } +void idPhysics_AI::SetWaterSurfaceWrldHeight(float) {} +float idPhysics_AI::GetWaterSurfaceWrldHeight() { return 0.0f; } + +void idPhysics_AI::GetImpactInfo(int, const idVec3*, + impactInfo_t* const info) { + if (info == nullptr) return; + info->Zero(); + info->invMass = invMass; + info->velocity = current.velocity; +} + +void idPhysics_AI::ApplyImpulse(int, const idVec3*, + const idVec3* const impulse) { + if (noImpact || impulse == nullptr) return; + if (swimmer && std::fabs(impulse->z) >= impulse->Length() - 1.0e-5f) + current.velocity.z += impulse->z * invMass * 1000.0f; + else + current.velocity = current.velocity + *impulse * invMass; + const float speed = current.velocity.Length(); + if (speed > 2000.0f) current.velocity = current.velocity + * (2000.0f / speed); + Activate(); +} + +void idPhysics_AI::ApplyForce(int id, const idVec3* point, + const idVec3* force) { + ApplyImpulse(id, point, force); +} + +void idPhysics_AI::Activate() { + current.atRest = -1; + if (callbacks != nullptr) + GameLib_NotifyPhysicsActivated(callbacks, GetPhysicsId()); +} +void idPhysics_AI::PutToRest() { + current.atRest = 1; + current.velocity.Zero(); +} +bool idPhysics_AI::IsAtRest() { return current.atRest >= 0; } + +void idPhysics_AI::SaveState() { + saved = current; + if (clipModels[0] != nullptr) saved.worldOrigin = clipModels[0]->GetOrigin(); +} +void idPhysics_AI::RestoreState() { + current = saved; + const idMat3 axis = clipModels[0] != nullptr + ? clipModels[0]->GetAxis() : clipModelAxis; + LinkClip(current.worldOrigin, axis); +} + +void idPhysics_AI::ResolveCollisions() { + lastCollisionNormal.Zero(); + lastCollisionPoint.Zero(); + ClearContacts(); + current.onGround = false; + idClipModel* const model = clipModels[0]; + if (model == nullptr) return; + AddGroundContacts(model, 12); + if (contacts.Num() == 0) return; + idVec3 normal; + normal.Zero(); + for (int index = 0; index < contacts.Num(); ++index) { + normal = normal + contacts[index].normal; + UpdateCollisionResidency(contacts[index]); + if (contacts[index].normal.Dot(gravityNormal) < -minFloorCosine) + current.onGround = true; + } + normal.NormalizeFast(); + lastCollisionNormal = normal; + lastCollisionPoint = contacts[0].point; + if (normal.LengthSqr() > 0.0f + && std::fabs(normal.Dot(gravityNormal)) < 0.9999f) { + stuckCollisionNormal = normal; + stuckCollisionPoint = lastCollisionPoint; + } +} + +void idPhysics_AI::Evolve(const float timeStep) { + idClipModel* const model = clipModels[0]; + if (model == nullptr || clip == nullptr || timeStep <= 0.0f) return; + float stepDown = 0.0f; + if (movementType == AI_MOVE_FLYING) { + stepDown = maxStepHeight; + } else if (movementType == AI_MOVE_DROPPING) { + current.velocity = current.velocity + gravityVector * timeStep; + if (current.velocity.z < maxDropVelocity) + current.velocity.z = maxDropVelocity; + stepDown = maxStepHeight; + } else if (current.onGround + && (!swimmer || GetWaterLevel(0) <= 0.0f)) { + ProjectOntoPlane(current.velocity, gravityNormal); + stepDown = maxStepHeight * 2.0f; + } else { + current.velocity = current.velocity + gravityVector * timeStep; + stepDown = maxStepHeight * 2.0f; + } + if (swimmer && GetWaterLevel(0) > 0.0f) { + const float damping = (std::max)(0.0f, 1.0f + - GetWaterViscosity(0) * GetWaterLevel(0) * timeStep); + current.velocity = current.velocity * damping; + } + ProjectOntoPlane(current.velocity, lastCollisionNormal, 1.001f); + const idVec3 start = model->GetOrigin(); + const idVec3 end = start + current.velocity * timeStep; + trace_t collision{}; + collision.fraction = 1.0f; + collision.endpos = end; + collision.endAxis = model->GetAxis(); + contactsResult_t contactResults{}; + stepMoveQuery = clip->StepMoveContacts(&collision, &contactResults, + start, end, gravityNormal, maxStepHeight, stepDown, model, + model->GetAxis(), clipMasks[0], GetEntityNumber(), false, + "idPhysics_AI::Evolve"); + current.worldOrigin = collision.endpos; + LinkClip(current.worldOrigin, collision.endAxis); + ClearContacts(); + const int count = (std::min)(contactResults.numContacts, 12); + for (int index = 0; index < count; ++index) { + contacts.Append(contactResults.contacts[index]); + UpdateCollisionResidency(contactResults.contacts[index]); + } + AddContactPhysicsForContacts(); + lastCollisionNormal = collision.c.normal; + lastCollisionPoint = collision.c.point; + current.onGround = false; + for (int index = 0; index < contacts.Num(); ++index) + if (contacts[index].normal.Dot(gravityNormal) < -minFloorCosine) + current.onGround = true; + if (collision.fraction < 1.0f) { + ProjectOntoPlane(current.velocity, collision.c.normal, 1.001f); + if (callbacks != nullptr) + GameLib_NotifyPhysicsCollision(callbacks, GetPhysicsId(), + collision, current.velocity); + } +} + +bool idPhysics_AI::Evaluate(const int timeStepMSec, int) { + RememberHistorySample(); + idClipModel* desired = crouched && canSetCrouchedPhysics + ? clipModel_crouched : clipModel_standing; + if (desired != nullptr && desired != clipModels[0]) { + const idVec3 origin = current.worldOrigin; + const idMat3 axis = clipModels[0] != nullptr + ? clipModels[0]->GetAxis() : clipModelAxis; + if (clipModels[0] != nullptr) clipModels[0]->Unlink(); + clipModels[0] = desired; + LinkClip(origin, axis); + } + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (!GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) return false; + const idVec3 oldOrigin = current.worldOrigin; + current.worldOrigin = masterOrigin + masterAxis * current.localOrigin; + const idMat3 axis = clipModels[0] != nullptr + ? clipModels[0]->GetAxis() : clipModelAxis; + LinkClip(current.worldOrigin, axis); + const float seconds = timeStepMSec * 0.001f; + if (seconds > 0.0f && masterControlledVelocity) + current.velocity = (current.worldOrigin - oldOrigin) + * (1.0f / seconds); + masterDeltaYaw = AxisYaw(masterAxis) - masterYaw; + masterYaw += masterDeltaYaw; + return (current.worldOrigin - oldOrigin).LengthSqr() != 0.0f; + } + if (IsAtRest()) return false; + ResolveCollisions(); + Evolve(timeStepMSec * 0.001f); + if (IsOutsideWorld() && callbacks != nullptr) + GameLib_NotifyPhysicsDeactivated(callbacks, GetPhysicsId()); + return true; +} + +void idPhysics_AI::UpdateTime(int) {} +void idPhysics_AI::SetPushed(const int deltaTime) { + if (deltaTime <= 0) { + current.pushVelocity.Zero(); + return; + } + current.pushVelocity = (current.worldOrigin - saved.worldOrigin) + * (1000.0f / static_cast(deltaTime)); +} +idVec3* idPhysics_AI::GetPushedLinearVelocity(idVec3* const result, int) { + if (result != nullptr) *result = current.pushVelocity; + return result; +} +idVec3* idPhysics_AI::GetPushedAngularVelocity(idVec3* const result, int) { + if (result != nullptr) result->Zero(); + return result; +} + +void idPhysics_AI::SetMaster(const bool enable, + const idVec3* const masterOrigin, const idMat3* const masterAxis, + bindFlags_t) { + stepMoveQuery.index = 0; + if (enable && masterOrigin != nullptr && masterAxis != nullptr) { + if (!hasMaster) { + current.localOrigin = masterAxis->Transpose() + * (current.worldOrigin - *masterOrigin); + masterYaw = AxisYaw(*masterAxis); + } + hasMaster = true; + ClearContacts(); + } else if (hasMaster) { + hasMaster = false; + Activate(); + } +} + +void idPhysics_AI::SetLocalOrigin(const idVec3* const newOrigin, int) { + if (newOrigin == nullptr) return; + stepMoveQuery.index = 0; + current.localOrigin = *newOrigin; + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + LinkClip(masterOrigin + masterAxis * *newOrigin, + clipModels[0] != nullptr ? clipModels[0]->GetAxis() + : clipModelAxis); + } else { + LinkClip(*newOrigin, clipModels[0] != nullptr + ? clipModels[0]->GetAxis() : clipModelAxis); + } + Activate(); +} + +void idPhysics_AI::SetLocalAxis(const idMat3* const newAxis, int) { + if (newAxis == nullptr) return; + idMat3 worldAxis = *newAxis; + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) worldAxis *= masterAxis; + } + LinkClip(current.worldOrigin, worldAxis); + Activate(); +} + +int idPhysics_AI::GetBlockingEntityNum() { + return contacts.Num() != 0 ? contacts[0].entityNum : 0x1FFF; +} +int idPhysics_AI::GetLinearEndTime() { return 0; } +int idPhysics_AI::GetAngularEndTime() { return 0; } +bool idPhysics_AI::IsOutsideWorld() { + return idPhysics_DynamicBase::IsOutsideWorld(); +} +const idMat3* idPhysics_AI::GetGravityAxis() { return &clipModelAxis; } +void idPhysics_AI::DisableClip(const actorClipModel_t type_) { + idPhysics_Actor::DisableClip(type_); +} +void idPhysics_AI::EnableClip(const actorClipModel_t type_) { + idPhysics_Actor::EnableClip(type_); +} +void idPhysics_AI::LinkClip(const idVec3& origin, const idMat3& axis) { + current.worldOrigin = origin; + idPhysics_Actor::LinkClip(origin, axis); +} diff --git a/source/engine/gamelib/physics/physics_ai.h b/source/engine/gamelib/physics/physics_ai.h index b579d8f..afa63d6 100644 --- a/source/engine/gamelib/physics/physics_ai.h +++ b/source/engine/gamelib/physics/physics_ai.h @@ -1,124 +1,102 @@ #pragma once -// Reconstructed C++ declarations from IDA Local Types and PDB/DIA metadata. -// Original PDB header: w:\tech5\engine\gamelib\physics\physics_ai.h -// Recovered logical types: 2 -// Signatures retain Xbox 360 ABI evidence and may still require manual review. +#include "gamelib/physics/clip.h" +#include "gamelib/physics/physics_actor.h" +#include -// IDA Local Type ordinal 2324; PDB kind: enum. -enum idPhysics_AI::aiMovement_t : __int32 -{ - AI_MOVE_WALKING = 0x0, - AI_MOVE_FLYING = 0x1, - AI_MOVE_DROPPING = 0x2, - AI_MOVE_MAX = 0x3, +struct aiPState_t { + std::int16_t atRest; + bool onGround; + char pad; + idVec3 localOrigin; + idVec3 worldOrigin; + idVec3 velocity; + idVec3 pushVelocity; }; -// IDA Local Type ordinal 16704; PDB kind: class. -class __declspec(align(8)) idPhysics_AI : public idPhysics_Actor -{ +class idPhysics_AI : public idPhysics_Actor { public: - // Recovered virtual interface; IDA vtable ordinal 16705. - virtual ~idPhysics_AI(); - virtual void SetClipModel(idClipModel *, float, int, bool); - virtual idClipModel *GetClipModel(int); - virtual int GetNumClipModels(); - virtual void SetMass(float, int); - virtual float GetMass(int); - virtual void SetContents(int, int); - virtual int GetContents(int); - virtual void SetClipMask(int, int); - virtual int GetClipMask(int); - virtual const idBounds *GetBounds(int); - virtual const idBounds *GetAbsBounds(int); - virtual void SetOrigin(const idVec3 *, int); - virtual void SetAxis(const idMat3 *, int); - virtual void Translate(const idVec3 *, int); - virtual void Rotate(const idRotation *, int); - virtual const idVec3 *GetOrigin(int); - virtual const idMat3 *GetAxis(int); - virtual const idVec3 *GetLocalOrigin(int); - virtual const idMat3 *GetLocalAxis(int); - virtual void SetLinearVelocity(const idVec3 *, int); - virtual void SetAngularVelocity(const idVec3 *, int); - virtual idVec3 *GetLinearVelocity(idVec3 *result, int); - virtual idVec3 *GetAngularVelocity(idVec3 *result, int); - virtual void SetGravity(const idVec3 *); - virtual const idVec3 *GetGravity(); - virtual const idVec3 *GetGravityNormal(); - virtual void SetWaterLevel(float, int); - virtual float GetWaterLevel(int); - virtual void SetWaterViscosity(float, int); - virtual float GetWaterViscosity(int); - virtual void SetWaterEntNum(int); - virtual int GetWaterEntNum(); - virtual void SetWaterSurfaceWrldHeight(float); - virtual float GetWaterSurfaceWrldHeight(); - virtual void GetImpactInfo(const int, const idVec3 *, impactInfo_t *); - virtual void ApplyImpulse(const int, const idVec3 *, const idVec3 *); - virtual void ApplyForce(const int, const idVec3 *, const idVec3 *); - virtual void Activate(); - virtual void PutToRest(); - virtual bool IsAtRest(); - virtual bool IsPushable(int); - virtual void SaveState(); - virtual void RestoreState(); - virtual bool Evaluate(int, int); - virtual void UpdateTime(int); - virtual void ClipTranslation(trace_t *, const idVec3 *, const idClipModel *); - virtual void ClipRotation(trace_t *, const idRotation *, const idClipModel *); - virtual int ClipContents(const idClipModel *, int); - virtual void DisableClip(); - virtual void EnableClip(); - virtual void UnlinkClip(); - virtual void LinkClip(); - virtual bool EvaluateContacts(); - virtual int GetNumContacts(); - virtual const contactInfo_t *GetContact(int); - virtual void ClearContacts(); - virtual void AddContactPhysics(idPhysics *); - virtual void RemoveContactPhysics(idPhysics *); - virtual int GetNumContactPhysics(); - virtual idPhysics *GetContactPhysics(int); - virtual void ActivateContactPhysics(); - virtual bool HasGroundContacts(); - virtual bool IsGroundEntity(int); - virtual bool IsGroundClipModel(int, int); - virtual void SetPushed(int); - virtual idVec3 *GetPushedLinearVelocity(idVec3 *result, const int); - virtual idVec3 *GetPushedAngularVelocity(idVec3 *result, const int); - virtual void SetMaster(bool, const idVec3 *, const idMat3 *, const bindFlags_t); - virtual void SetLocalOrigin(const idVec3 *, int); - virtual void SetLocalAxis(const idMat3 *, int); - virtual int GetBlockingEntityNum(); - virtual int GetLinearEndTime(); - virtual int GetAngularEndTime(); - virtual bool IsInNonResidentCollisionArea(bool); - virtual bool IsOutsideWorld(); - virtual const idMat3 *GetGravityAxis(); - virtual void DisableClip_2(const idPhysics_Actor::actorClipModel_t); - virtual void EnableClip_2(const idPhysics_Actor::actorClipModel_t); - virtual void LinkClip_2(const idVec3 *, const idMat3 *); + enum aiMovement_t : int { + AI_MOVE_WALKING = 0, + AI_MOVE_FLYING = 1, + AI_MOVE_DROPPING = 2, + AI_MOVE_MAX = 3 + }; - aiPState_t current; - aiPState_t saved; - idClipQuery stepMoveQuery; - idClipQuery contactsQuery; - idVec3 lastCollisionNormal; - idVec3 lastCollisionPoint; - idVec3 stuckCollisionNormal; - idVec3 stuckCollisionPoint; - idClipModel *clipModel_standing; - idClipModel *clipModel_crouched; - float maxStepHeight; - float minFloorCosine; - float maxDropVelocity; - float heightCrouched; - idPhysics_AI::aiMovement_t movementType; - bool noImpact; - bool masterControlledVelocity; - bool swimmer; - bool crouched; - bool canSetCrouchedPhysics; + idPhysics_AI(); + ~idPhysics_AI() override; + + void SetClipModel(idClipModel*, float, int, bool) override; + void SetContents(int, int) override; + void SetOrigin(const idVec3*, int) override; + void SetAxis(const idMat3*, int) override; + void Translate(const idVec3*, int) override; + void Rotate(const idRotation*, int) override; + const idVec3* GetLocalOrigin(int) override; + const idMat3* GetLocalAxis(int) override; + void SetLinearVelocity(const idVec3*, int) override; + void SetAngularVelocity(const idVec3*, int) override; + idVec3* GetLinearVelocity(idVec3*, int) override; + idVec3* GetAngularVelocity(idVec3*, int) override; + void SetWaterEntNum(int) override; + int GetWaterEntNum() override; + void SetWaterSurfaceWrldHeight(float) override; + float GetWaterSurfaceWrldHeight() override; + void GetImpactInfo(int, const idVec3*, impactInfo_t*) override; + void ApplyImpulse(int, const idVec3*, const idVec3*) override; + void ApplyForce(int, const idVec3*, const idVec3*) override; + void Activate() override; + void PutToRest() override; + bool IsAtRest() override; + void SaveState() override; + void RestoreState() override; + bool Evaluate(int, int) override; + void UpdateTime(int) override; + void SetPushed(int) override; + idVec3* GetPushedLinearVelocity(idVec3*, int) override; + idVec3* GetPushedAngularVelocity(idVec3*, int) override; + void SetMaster(bool, const idVec3*, const idMat3*, bindFlags_t) override; + void SetLocalOrigin(const idVec3*, int) override; + void SetLocalAxis(const idMat3*, int) override; + int GetBlockingEntityNum() override; + int GetLinearEndTime() override; + int GetAngularEndTime() override; + + bool IsOutsideWorld() override; + const idMat3* GetGravityAxis() override; + void DisableClip(actorClipModel_t) override; + void EnableClip(actorClipModel_t) override; + void LinkClip(const idVec3&, const idMat3&) override; + + void Evolve(float timeStep); + void ResolveCollisions(); + + aiPState_t current; + aiPState_t saved; + idClipQuery stepMoveQuery; + idClipQuery contactsQuery; + idVec3 lastCollisionNormal; + idVec3 lastCollisionPoint; + idVec3 stuckCollisionNormal; + idVec3 stuckCollisionPoint; + idClipModel* clipModel_standing; + idClipModel* clipModel_crouched; + float maxStepHeight; + float minFloorCosine; + float maxDropVelocity; + float heightCrouched; + aiMovement_t movementType; + bool noImpact; + bool masterControlledVelocity; + bool swimmer; + bool crouched; + bool canSetCrouchedPhysics; }; + +static_assert(sizeof(aiPState_t) == 52, + "Recovered AI physics state ABI changed"); +#if defined(_WIN32) && !defined(_WIN64) +static_assert(sizeof(idPhysics_AI) == 664, + "Recovered idPhysics_AI ABI changed"); +#endif diff --git a/source/engine/gamelib/physics/physics_dynamicbase.h b/source/engine/gamelib/physics/physics_dynamicbase.h index da712f1..1e3f1f7 100644 --- a/source/engine/gamelib/physics/physics_dynamicbase.h +++ b/source/engine/gamelib/physics/physics_dynamicbase.h @@ -32,7 +32,7 @@ public: bool HasGroundContacts() override; bool IsGroundEntity(int entityNumber) override; bool IsGroundClipModel(int entityNumber, int bodyId) override; - bool IsOutsideWorld(); + virtual bool IsOutsideWorld(); protected: void AddContactPhysicsForContacts(); diff --git a/source/engine/gamelib/physics/physics_parametric.cpp b/source/engine/gamelib/physics/physics_parametric.cpp new file mode 100644 index 0000000..eb4d57c --- /dev/null +++ b/source/engine/gamelib/physics/physics_parametric.cpp @@ -0,0 +1,923 @@ +#include "gamelib/physics/physics_parametric.h" + +#include "gamelib/physics/clip.h" +#include "gamelib/physics/clipmodel.h" +#include "gamelib/physics/push.h" +#include "idlib/math/rotation.h" + +#include +#include +#include + +bool GameLib_GetMasterPhysicsTransform(idPhysicsCallbacks* callbacks, + idVec3& origin, idMat3& axis); +void GameLib_NotifyPhysicsActivated(idPhysicsCallbacks* callbacks, + int physicsId); + +namespace { + +const idBounds kParametricZeroBounds{{idVec3(0.0f, 0.0f, 0.0f), + idVec3(0.0f, 0.0f, 0.0f)}}; +const idVec3 kParametricZeroVector(0.0f, 0.0f, 0.0f); +const idMat3 kParametricIdentityAxis(1.0f); + +void ZeroSpatial(idVec6& value) { + for (int index = 0; index < 6; ++index) value[index] = 0.0f; +} +void SetSpatial(idVec6& value, const idVec3& linear, + const idVec3& angular) { + value[0] = linear.x; value[1] = linear.y; value[2] = linear.z; + value[3] = angular.x; value[4] = angular.y; value[5] = angular.z; +} +idVec3 SpatialLinear(const idVec6& value) { + return idVec3(value[0], value[1], value[2]); +} +idVec3 SpatialAngular(const idVec6& value) { + return idVec3(value[3], value[4], value[5]); +} + +idAngles Mat3ToAngles(const idMat3& axis) { + idAngles angles; + const float forwardLength = std::sqrt(axis[0].x * axis[0].x + + axis[0].y * axis[0].y); + angles.pitch = std::atan2(-axis[0].z, forwardLength) + * 57.29577951308232f; + angles.yaw = std::atan2(axis[0].y, axis[0].x) + * 57.29577951308232f; + angles.roll = std::atan2(-axis[1].z, axis[2].z) + * 57.29577951308232f; + return angles; +} + +idQuat Mat3ToQuat(const idMat3& matrix) { + idQuat result; + const float trace = matrix[0].x + matrix[1].y + matrix[2].z; + if (trace > 0.0f) { + const float scale = std::sqrt(trace + 1.0f) * 2.0f; + result.w = 0.25f * scale; + result.x = (matrix[2].y - matrix[1].z) / scale; + result.y = (matrix[0].z - matrix[2].x) / scale; + result.z = (matrix[1].x - matrix[0].y) / scale; + } else if (matrix[0].x > matrix[1].y + && matrix[0].x > matrix[2].z) { + const float scale = std::sqrt(1.0f + matrix[0].x + - matrix[1].y - matrix[2].z) * 2.0f; + result.w = (matrix[2].y - matrix[1].z) / scale; + result.x = 0.25f * scale; + result.y = (matrix[0].y + matrix[1].x) / scale; + result.z = (matrix[0].z + matrix[2].x) / scale; + } else if (matrix[1].y > matrix[2].z) { + const float scale = std::sqrt(1.0f + matrix[1].y + - matrix[0].x - matrix[2].z) * 2.0f; + result.w = (matrix[0].z - matrix[2].x) / scale; + result.x = (matrix[0].y + matrix[1].x) / scale; + result.y = 0.25f * scale; + result.z = (matrix[1].z + matrix[2].y) / scale; + } else { + const float scale = std::sqrt(1.0f + matrix[2].z + - matrix[0].x - matrix[1].y) * 2.0f; + result.w = (matrix[1].x - matrix[0].y) / scale; + result.x = (matrix[0].z + matrix[2].x) / scale; + result.y = (matrix[1].z + matrix[2].y) / scale; + result.z = 0.25f * scale; + } + result.Normalize(); + return result; +} + +idMat3 QuatToMat3(idQuat quat) { + quat.Normalize(); + const float xx = quat.x * quat.x; + const float yy = quat.y * quat.y; + const float zz = quat.z * quat.z; + const float xy = quat.x * quat.y; + const float xz = quat.x * quat.z; + const float yz = quat.y * quat.z; + const float wx = quat.w * quat.x; + const float wy = quat.w * quat.y; + const float wz = quat.w * quat.z; + return idMat3( + 1.0f - 2.0f * (yy + zz), 2.0f * (xy - wz), + 2.0f * (xz + wy), 2.0f * (xy + wz), + 1.0f - 2.0f * (xx + zz), 2.0f * (yz - wx), + 2.0f * (xz - wy), 2.0f * (yz + wx), + 1.0f - 2.0f * (xx + yy)); +} + +bool MatrixChanged(const idMat3& first, const idMat3& second) { + return (first[0] - second[0]).LengthSqr() != 0.0f + || (first[1] - second[1]).LengthSqr() != 0.0f + || (first[2] - second[2]).LengthSqr() != 0.0f; +} + +} // namespace + +parametricPState_t::parametricPState_t() + : time(0) + , atRest(-1) + , worldOrigin(0.0f, 0.0f, 0.0f) + , worldAngles(0.0f, 0.0f, 0.0f) + , worldAxis(1.0f) + , localOrigin(0.0f, 0.0f, 0.0f) + , localAngles(0.0f, 0.0f, 0.0f) + , linearExtrapolation() + , angularExtrapolation() + , linearInterpolation() + , angularInterpolation() + , spline(nullptr) + , angularSpline(nullptr) + , splineInterpolate() + , useSplineAngles(false) { + linearInterpolation.Init(0.0f, 0.0f, 0.0f, 0.0f, + kParametricZeroVector, kParametricZeroVector); + angularInterpolation.Init(0.0f, 0.0f, 0.0f, 0.0f, + idAngles(0.0f, 0.0f, 0.0f), idAngles(0.0f, 0.0f, 0.0f)); + splineInterpolate.Init(0.0f, 1.0f, 1.0f, 2.0f, 0.0f, 0.0f); +} + +idPhysics_Parametric::idPhysics_Parametric() + : idPhysics_DynamicBase() + , current() + , saved(current) + , spatialVelocity{} + , blockingPhysicsId(-1) + , absBounds(kParametricZeroBounds) + , pusher(nullptr) + , isPusher(false) + , clipModel(nullptr) + , pushFlags(0) + , hasMaster(false) + , isOrientated(false) + , hasWorldOrientation(false) + , worldAxis(1.0f) + , worldOrigin(0.0f, 0.0f, 0.0f) { + type = PHYSICS_PARAMETRIC; + ZeroSpatial(spatialVelocity); +} + +idPhysics_Parametric::~idPhysics_Parametric() { + if (clipModel != nullptr) clipModel->Unlink(); + clipModel = nullptr; +} + +void idPhysics_Parametric::SetPusher(idPush* const push, const int flags) { + pusher = push; + isPusher = push != nullptr; + pushFlags = flags; +} +bool idPhysics_Parametric::IsPusher() const { return isPusher; } +idCurve_Spline* idPhysics_Parametric::GetSpline() { + return current.spline; +} +const idAngles* idPhysics_Parametric::GetLocalAngles() const { + return ¤t.localAngles; +} + +void idPhysics_Parametric::SetClipModel(idClipModel* const model, float, + int, const bool freeOld) { + if (clipModel != nullptr && clipModel != model && freeOld) + clipModel->Delete(); + clipModel = model; + LinkClip(); +} +idClipModel* idPhysics_Parametric::GetClipModel(int) { return clipModel; } +int idPhysics_Parametric::GetNumClipModels() { return clipModel != nullptr; } +void idPhysics_Parametric::SetMass(float, int) {} +float idPhysics_Parametric::GetMass(int) { return 0.0f; } +void idPhysics_Parametric::SetContents(const int contents, int) { + if (clipModel != nullptr) clipModel->SetContents(contents); +} +int idPhysics_Parametric::GetContents(int) { + return clipModel != nullptr ? clipModel->GetContents() : 0; +} +const idBounds* idPhysics_Parametric::GetBounds(int) { + return clipModel != nullptr ? &clipModel->GetBounds() + : &kParametricZeroBounds; +} +const idBounds* idPhysics_Parametric::GetAbsBounds(int) { + if (clipModel != nullptr) return &clipModel->GetAbsBounds(); + absBounds[0] = current.worldOrigin; + absBounds[1] = current.worldOrigin; + return &absBounds; +} + +void idPhysics_Parametric::SetLinearExtrapolation( + const extrapolation_t extrapolation, const int currentTime, + const int startTime, const int duration, const idVec3& base, + const idVec3& baseSpeed, const idVec3& speed) { + current.time = currentTime; + current.linearExtrapolation.Init(static_cast(startTime), + static_cast(duration), base, baseSpeed, speed, extrapolation); + current.localOrigin = base; + Activate(); +} +void idPhysics_Parametric::SetAngularExtrapolation( + const extrapolation_t extrapolation, const int currentTime, + const int startTime, const int duration, const idAngles& base, + const idAngles& baseSpeed, const idAngles& speed) { + current.time = currentTime; + current.angularExtrapolation.Init(static_cast(startTime), + static_cast(duration), base, baseSpeed, speed, extrapolation); + current.localAngles = base; + Activate(); +} +idAngles idPhysics_Parametric::GetCurrentAngularExtrapolationAngles( + const int time) const { + return current.angularExtrapolation.GetCurrentValue( + static_cast(time)); +} +void idPhysics_Parametric::SetLinearInterpolation(const int currentTime, + const int startTime, const int accelTime, const int decelTime, + const int duration, const idVec3& start, const idVec3& end) { + current.time = currentTime; + current.linearInterpolation.Init(static_cast(startTime), + static_cast(accelTime), static_cast(decelTime), + static_cast(duration), start, end); + current.localOrigin = start; + Activate(); +} +void idPhysics_Parametric::SetAngularInterpolation(const int currentTime, + const int startTime, const int accelTime, const int decelTime, + const int duration, const idAngles& start, const idAngles& end) { + current.time = currentTime; + current.angularInterpolation.Init(static_cast(startTime), + static_cast(accelTime), static_cast(decelTime), + static_cast(duration), start, end); + current.localAngles = start; + Activate(); +} +void idPhysics_Parametric::SetSpline(idCurve_Spline* const spline, + const int accelTime, const int decelTime, + const bool useAngles, + idCurve_Spline* const angularSpline) { + current.spline = spline; + current.angularSpline = angularSpline; + current.useSplineAngles = useAngles; + if (spline != nullptr && spline->GetNumValues() > 0) { + const float start = spline->GetTime(0); + const float end = spline->GetTime(spline->GetNumValues() - 1); + const float length = spline->GetLengthForTime(end); + current.splineInterpolate.Init(start, + static_cast(accelTime), static_cast(decelTime), + end - start, 0.0f, length); + } + Activate(); +} + +void idPhysics_Parametric::SetOrigin(const idVec3* const origin, int) { + if (origin == nullptr) return; + current.localOrigin = *origin; + current.worldOrigin = *origin; + current.linearExtrapolation.SetStartValue(*origin); + current.linearInterpolation.SetStartValue(*origin); + current.linearInterpolation.SetEndValue(*origin); + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) { + current.localOrigin = masterAxis.Transpose() + * (*origin - masterOrigin); + current.worldOrigin = *origin; + } + } + LinkClip(); + Activate(); +} +void idPhysics_Parametric::SetAxis(const idMat3* const axis, int) { + if (axis == nullptr) return; + current.worldAxis = *axis; + current.worldAngles = Mat3ToAngles(*axis); + current.localAngles = current.worldAngles; + current.angularExtrapolation.SetStartValue(current.localAngles); + current.angularInterpolation.SetStartValue(current.localAngles); + current.angularInterpolation.SetEndValue(current.localAngles); + LinkClip(); + Activate(); +} +void idPhysics_Parametric::Translate(const idVec3* const translation, int) { + if (translation == nullptr) return; + const idVec3 target = current.worldOrigin + *translation; + SetOrigin(&target, 0); +} +void idPhysics_Parametric::Rotate(const idRotation* const rotation, int) { + if (rotation == nullptr) return; + current.worldOrigin = *rotation * current.worldOrigin; + current.worldAxis *= rotation->ToMat3(); + current.worldAngles = Mat3ToAngles(current.worldAxis); + current.localAngles = current.worldAngles; + LinkClip(); + Activate(); +} +const idVec3* idPhysics_Parametric::GetOrigin(int) { + return ¤t.worldOrigin; +} +const idMat3* idPhysics_Parametric::GetAxis(int) { + return ¤t.worldAxis; +} +const idVec3* idPhysics_Parametric::GetLocalOrigin(int) { + return ¤t.localOrigin; +} +const idMat3* idPhysics_Parametric::GetLocalAxis(int) { + static idMat3 localAxis; + localAxis = current.localAngles.ToMat3(); + return &localAxis; +} +void idPhysics_Parametric::SetLinearVelocity(const idVec3* const velocity, + int) { + if (velocity == nullptr) return; + SetLinearExtrapolation(static_cast( + EXTRAPOLATION_LINEAR | EXTRAPOLATION_NOSTOP), + current.time, current.time, 0, current.localOrigin, + kParametricZeroVector, *velocity); +} +void idPhysics_Parametric::SetAngularVelocity(const idVec3* const velocity, + int) { + if (velocity == nullptr) return; + const idAngles angularSpeed(velocity->x, velocity->y, velocity->z); + SetAngularExtrapolation(static_cast( + EXTRAPOLATION_LINEAR | EXTRAPOLATION_NOSTOP), + current.time, current.time, 0, current.localAngles, + idAngles(0.0f, 0.0f, 0.0f), angularSpeed); +} +idVec3* idPhysics_Parametric::GetLinearVelocity(idVec3* const result, int) { + if (result != nullptr) *result = SpatialLinear(spatialVelocity); + return result; +} +idVec3* idPhysics_Parametric::GetAngularVelocity(idVec3* const result, int) { + if (result != nullptr) *result = SpatialAngular(spatialVelocity); + return result; +} +void idPhysics_Parametric::SetWaterEntNum(int) {} +int idPhysics_Parametric::GetWaterEntNum() { return -1; } +void idPhysics_Parametric::SetWaterSurfaceWrldHeight(float) {} +float idPhysics_Parametric::GetWaterSurfaceWrldHeight() { return 0.0f; } +void idPhysics_Parametric::GetImpactInfo(int, const idVec3*, + impactInfo_t* const info) { + if (info == nullptr) return; + info->Zero(); + info->velocity = SpatialLinear(spatialVelocity); +} +void idPhysics_Parametric::ApplyImpulse(int, const idVec3*, const idVec3*) {} +void idPhysics_Parametric::ApplyForce(int, const idVec3*, const idVec3*) {} +void idPhysics_Parametric::Activate() { + current.atRest = -1; + if (callbacks != nullptr) + GameLib_NotifyPhysicsActivated(callbacks, GetPhysicsId()); +} +void idPhysics_Parametric::PutToRest() { + current.atRest = current.time; + ZeroSpatial(spatialVelocity); +} +bool idPhysics_Parametric::IsAtRest() { return current.atRest >= 0; } +bool idPhysics_Parametric::IsPushable(int) { return false; } +void idPhysics_Parametric::SaveState() { saved = current; } +void idPhysics_Parametric::RestoreState() { + current = saved; + LinkClip(); +} + +void idPhysics_Parametric::ForceUpdateSpatialVelocity( + const int timeStepMSec) { + if (timeStepMSec <= 0) return; + const float inverseSeconds = 1000.0f / static_cast(timeStepMSec); + const idVec3 oldOrigin = current.worldOrigin; + const idAngles oldAngles = current.worldAngles; + const float sampleTime = static_cast(current.time + timeStepMSec); + idVec3 newOrigin = current.linearExtrapolation.GetCurrentValue(sampleTime); + if (current.linearInterpolation.GetDuration() > 0.0f) + newOrigin = current.linearInterpolation.GetCurrentValue(sampleTime); + idAngles newAngles = current.angularExtrapolation.GetCurrentValue(sampleTime); + if (current.angularInterpolation.GetDuration() > 0.0f) + newAngles = current.angularInterpolation.GetCurrentValue(sampleTime); + SetSpatial(spatialVelocity, (newOrigin - oldOrigin) * inverseSeconds, + idVec3(newAngles.pitch - oldAngles.pitch, + newAngles.yaw - oldAngles.yaw, + newAngles.roll - oldAngles.roll) * inverseSeconds); +} + +bool idPhysics_Parametric::Evaluate(int timeStepMSec, + const int endTimeMSec) { + const idVec3 oldOrigin = current.worldOrigin; + const idMat3 oldAxis = current.worldAxis; + current.time = endTimeMSec; + idVec3 localOrigin = current.linearExtrapolation.GetCurrentValue( + static_cast(endTimeMSec)); + if (current.linearInterpolation.GetDuration() > 0.0f) + localOrigin = current.linearInterpolation.GetCurrentValue( + static_cast(endTimeMSec)); + idAngles localAngles = current.angularExtrapolation.GetCurrentValue( + static_cast(endTimeMSec)); + if (current.angularInterpolation.GetDuration() > 0.0f) + localAngles = current.angularInterpolation.GetCurrentValue( + static_cast(endTimeMSec)); + if (current.spline != nullptr && current.spline->GetNumValues() > 0) { + const float length = current.splineInterpolate.GetCurrentValue( + static_cast(endTimeMSec)); + const float splineTime = current.spline->GetTimeForLength(length); + localOrigin = current.spline->GetCurrentValue(splineTime); + if (current.angularSpline != nullptr) + localAngles = current.angularSpline->GetCurrentValue(splineTime); + } + current.localOrigin = localOrigin; + current.localAngles = localAngles; + idVec3 desiredOrigin = localOrigin; + idMat3 desiredAxis = localAngles.ToMat3(); + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) { + desiredOrigin = masterOrigin + masterAxis * localOrigin; + if (isOrientated) desiredAxis *= masterAxis; + } + } + if (hasWorldOrientation) { + desiredOrigin = worldOrigin + worldAxis * desiredOrigin; + desiredAxis *= worldAxis; + } + blockingPhysicsId = -1; + if (isPusher && pusher != nullptr) { + trace_t pushTrace{}; + pusher->ClipPush(pushTrace, this, pushFlags, oldOrigin, oldAxis, + desiredOrigin, desiredAxis); + if (pushTrace.fraction < 1.0f) + blockingPhysicsId = pushTrace.c.physicsId; + } + current.worldOrigin = desiredOrigin; + current.worldAngles = Mat3ToAngles(desiredAxis); + current.worldAxis = desiredAxis; + if (timeStepMSec > 0) + SetSpatial(spatialVelocity, + (current.worldOrigin - oldOrigin) + * (1000.0f / static_cast(timeStepMSec)), + idVec3(current.worldAngles.pitch - Mat3ToAngles(oldAxis).pitch, + current.worldAngles.yaw - Mat3ToAngles(oldAxis).yaw, + current.worldAngles.roll - Mat3ToAngles(oldAxis).roll) + * (1000.0f / static_cast(timeStepMSec))); + LinkClip(); + const bool linearDone = current.linearInterpolation.GetDuration() > 0.0f + ? current.linearInterpolation.IsDone(static_cast(endTimeMSec)) + : current.linearExtrapolation.IsDone(static_cast(endTimeMSec)); + const bool angularDone = current.angularInterpolation.GetDuration() > 0.0f + ? current.angularInterpolation.IsDone(static_cast(endTimeMSec)) + : current.angularExtrapolation.IsDone(static_cast(endTimeMSec)); + const bool splineDone = current.spline == nullptr + || current.splineInterpolate.IsDone(static_cast(endTimeMSec)); + if (linearDone && angularDone && splineDone) PutToRest(); + return (current.worldOrigin - oldOrigin).LengthSqr() != 0.0f + || MatrixChanged(current.worldAxis, oldAxis); +} + +void idPhysics_Parametric::UpdateTime(const int endTimeMSec) { + const float delta = static_cast(endTimeMSec - current.time); + current.time = endTimeMSec; + current.linearExtrapolation.SetStartTime( + current.linearExtrapolation.GetStartTime() + delta); + current.angularExtrapolation.SetStartTime( + current.angularExtrapolation.GetStartTime() + delta); + current.linearInterpolation.SetStartTime( + current.linearInterpolation.GetStartTime() + delta); + current.angularInterpolation.SetStartTime( + current.angularInterpolation.GetStartTime() + delta); + current.splineInterpolate.SetStartTime( + current.splineInterpolate.GetStartTime() + delta); + if (current.spline != nullptr) current.spline->ShiftTime(delta); + if (current.angularSpline != nullptr) current.angularSpline->ShiftTime(delta); +} + +void idPhysics_Parametric::ClipRotation(trace_t* const results, + const idRotation* const rotation, const idClipModel* const model) { + if (results == nullptr) return; + const idClipModel* moving = model != nullptr ? model : clipModel; + if (clip == nullptr || moving == nullptr || rotation == nullptr) { + std::memset(results, 0, sizeof(*results)); + results->fraction = 1.0f; + return; + } + clip->Rotation(results, current.worldOrigin, *rotation, moving, + current.worldAxis, clipMask, GetEntityNumber(), false, + "idPhysics_Parametric::ClipRotation"); +} +int idPhysics_Parametric::ClipContents(const idClipModel* const model, + int queryMask) { + if (clip == nullptr || clipModel == nullptr) return 0; + if (queryMask == 0) queryMask = clipMask; + trace_t result{}; + if (model != nullptr) + clip->ContentsModel(result, current.worldOrigin, clipModel, + current.worldAxis, queryMask, model->GetOrigin(), model, + model->GetAxis()); + else + clip->Contents(&result, current.worldOrigin, clipModel, + current.worldAxis, queryMask, GetEntityNumber(), + "idPhysics_Parametric::ClipContents"); + return result.c.contentFlags; +} +void idPhysics_Parametric::DisableClip() { + if (clipModel != nullptr) clipModel->Disable(); +} +void idPhysics_Parametric::EnableClip() { + if (clipModel != nullptr) clipModel->Enable(); +} +void idPhysics_Parametric::UnlinkClip() { + if (clipModel != nullptr) clipModel->Unlink(); +} +void idPhysics_Parametric::LinkClip() { + if (clipModel != nullptr) + clipModel->Link(GetEntityNumber(), GetEntityNumber(), 0, + current.worldOrigin, current.worldAxis); +} +bool idPhysics_Parametric::EvaluateContacts() { return false; } +void idPhysics_Parametric::SetPushed(int) {} +idVec3* idPhysics_Parametric::GetPushedLinearVelocity(idVec3* result, int) { + if (result != nullptr) result->Zero(); return result; +} +idVec3* idPhysics_Parametric::GetPushedAngularVelocity(idVec3* result, int) { + if (result != nullptr) result->Zero(); return result; +} +void idPhysics_Parametric::SetMaster(const bool enable, + const idVec3* const masterOrigin, const idMat3* const masterAxis, + const bindFlags_t flags) { + if (enable && masterOrigin != nullptr && masterAxis != nullptr) { + current.localOrigin = masterAxis->Transpose() + * (current.worldOrigin - *masterOrigin); + current.localAngles = Mat3ToAngles((static_cast(flags) & 1) != 0 + ? current.worldAxis * masterAxis->Transpose() + : current.worldAxis); + hasMaster = true; + isOrientated = (static_cast(flags) & 1) != 0; + } else { + hasMaster = false; + } +} +void idPhysics_Parametric::SetLocalOrigin(const idVec3* origin, int) { + if (origin == nullptr) return; + current.localOrigin = *origin; + current.linearExtrapolation.SetStartValue(*origin); + Activate(); +} +void idPhysics_Parametric::SetLocalAxis(const idMat3* axis, int) { + if (axis == nullptr) return; + current.localAngles = Mat3ToAngles(*axis); + current.angularExtrapolation.SetStartValue(current.localAngles); + Activate(); +} +int idPhysics_Parametric::GetBlockingEntityNum() { + idPhysics* physics = idPhysics::GetPhysicsForId(blockingPhysicsId); + return physics != nullptr ? physics->GetEntityNumber() : 0x1FFF; +} +int idPhysics_Parametric::GetLinearEndTime() { + if (current.linearInterpolation.GetDuration() > 0.0f) + return static_cast(current.linearInterpolation.GetEndTime()); + return (static_cast(current.linearExtrapolation.extrapolationType) + & EXTRAPOLATION_NOSTOP) != 0 ? 0 + : static_cast(current.linearExtrapolation.GetEndTime()); +} +int idPhysics_Parametric::GetAngularEndTime() { + if (current.angularInterpolation.GetDuration() > 0.0f) + return static_cast(current.angularInterpolation.GetEndTime()); + return (static_cast(current.angularExtrapolation.extrapolationType) + & EXTRAPOLATION_NOSTOP) != 0 ? 0 + : static_cast(current.angularExtrapolation.GetEndTime()); +} +bool idPhysics_Parametric::IsOutsideWorld() { + return idPhysics_DynamicBase::IsOutsideWorld(); +} +void idPhysics_Parametric::SetWorldOrientation(const idVec3& origin, + const idMat3& axis) { + if (hasWorldOrientation) ClearWorldOrientation(); + worldOrigin = origin; + worldAxis = axis; + hasWorldOrientation = true; +} +void idPhysics_Parametric::ClearWorldOrientation() { + hasWorldOrientation = false; + worldOrigin.Zero(); + worldAxis = idMat3(1.0f); +} +extrapolation_t idPhysics_Parametric::GetAngularExtrapolationType() const { + return current.angularExtrapolation.extrapolationType; +} + +// Motion-matching parametric implementation. Its authority uses the same +// physics contract but stores quaternion interpolation and explicit velocity. +parametricPState_tMM::parametricPState_tMM() + : time(0), length(0.0f), totalLength(0.0f), atRest(-1), + worldOrigin(0.0f, 0.0f, 0.0f), worldAxis(1.0f), + localOrigin(0.0f, 0.0f, 0.0f), localAxis(1.0f), + linearExtrapolation(), angularExtrapolation(), + angularExtrapolationStartAxis(1.0f), + angularExtrapolationRotVec(0.0f, 0.0f, 1.0f), + linearVelocity(0.0f, 0.0f, 0.0f), + angularVelocity(0.0f, 0.0f, 0.0f), linearInterpolation(), + angularInterpolation(), splineInterpolate(), + splineAngleInterpolate(), spline(nullptr), + splineDerivative(0.0f, 0.0f, 0.0f), useSplineAngles(false), + pauseTime(0), oscillationExtrapolation{}, + oscillationZRelative(false), localOffsetInterpolation(), + splineMaster(nullptr), localOriginScaleInterpolation(), + splineChilds(16), splineDummy(false) { +} + +idPhysics_ParametricMM::idPhysics_ParametricMM() + : idPhysics_DynamicBase(), current(), saved(current), + blockingPhysicsId(-1), absBounds(kParametricZeroBounds), pusher(nullptr), + isPusher(false), clipModel(nullptr), pushFlags(0), hasMaster(false), + isOrientated(false), hasWorldOrientation(false), worldAxis(1.0f), + worldOrigin(0.0f, 0.0f, 0.0f), collideClipMask(0), + collideCallbackEnabled(false) { + type = PHYSICS_PARAMETRIC; +} +idPhysics_ParametricMM::~idPhysics_ParametricMM() { + if (clipModel != nullptr) clipModel->Unlink(); + clipModel = nullptr; +} +void idPhysics_ParametricMM::SetPusher(idPush* push, int flags) { + pusher = push; isPusher = push != nullptr; pushFlags = flags; +} +idCurve_Spline* idPhysics_ParametricMM::GetSpline() { + return current.spline; +} +void idPhysics_ParametricMM::SetUseSplineAngles(bool value) { + current.useSplineAngles = value; +} +void idPhysics_ParametricMM::PauseParametricUntil(int resumeTime) { + current.pauseTime = resumeTime; +} +void idPhysics_ParametricMM::SetClipModel(idClipModel* model, float, int, + bool freeOld) { + if (clipModel != nullptr && clipModel != model && freeOld) + clipModel->Delete(); + clipModel = model; LinkClip(); +} +idClipModel* idPhysics_ParametricMM::GetClipModel(int) { return clipModel; } +int idPhysics_ParametricMM::GetNumClipModels() { return clipModel != nullptr; } +void idPhysics_ParametricMM::SetMass(float, int) {} +float idPhysics_ParametricMM::GetMass(int) { return 0.0f; } +void idPhysics_ParametricMM::SetContents(int value, int) { + if (clipModel != nullptr) clipModel->SetContents(value); +} +int idPhysics_ParametricMM::GetContents(int) { + return clipModel != nullptr ? clipModel->GetContents() : 0; +} +const idBounds* idPhysics_ParametricMM::GetBounds(int) { + return clipModel != nullptr ? &clipModel->GetBounds() + : &kParametricZeroBounds; +} +const idBounds* idPhysics_ParametricMM::GetAbsBounds(int) { + return clipModel != nullptr ? &clipModel->GetAbsBounds() + : &kParametricZeroBounds; +} +void idPhysics_ParametricMM::SetOrigin(const idVec3* value, int) { + if (value == nullptr) return; + current.worldOrigin = *value; current.localOrigin = *value; + current.linearExtrapolation.SetStartValue(*value); LinkClip(); Activate(); +} +void idPhysics_ParametricMM::SetAxis(const idMat3* value, int) { + if (value == nullptr) return; + current.worldAxis = *value; current.localAxis = *value; + LinkClip(); Activate(); +} +void idPhysics_ParametricMM::Translate(const idVec3* value, int) { + if (value == nullptr) return; + current.worldOrigin = current.worldOrigin + *value; + current.localOrigin = current.localOrigin + *value; LinkClip(); Activate(); +} +void idPhysics_ParametricMM::Rotate(const idRotation* value, int) { + if (value == nullptr) return; + current.worldOrigin = *value * current.worldOrigin; + current.worldAxis *= value->ToMat3(); + current.localAxis *= value->ToMat3(); LinkClip(); Activate(); +} +const idVec3* idPhysics_ParametricMM::GetOrigin(int) { + return ¤t.worldOrigin; +} +const idMat3* idPhysics_ParametricMM::GetAxis(int) { return ¤t.worldAxis; } +const idVec3* idPhysics_ParametricMM::GetLocalOrigin(int) { + return ¤t.localOrigin; +} +const idMat3* idPhysics_ParametricMM::GetLocalAxis(int) { + return ¤t.localAxis; +} +void idPhysics_ParametricMM::SetLinearVelocity(const idVec3* value, int) { + if (value != nullptr) { current.linearVelocity = *value; Activate(); } +} +void idPhysics_ParametricMM::SetAngularVelocity(const idVec3* value, int) { + if (value != nullptr) { current.angularVelocity = *value; Activate(); } +} +idVec3* idPhysics_ParametricMM::GetLinearVelocity(idVec3* result, int) { + if (result != nullptr) *result = current.linearVelocity; return result; +} +idVec3* idPhysics_ParametricMM::GetAngularVelocity(idVec3* result, int) { + if (result != nullptr) *result = current.angularVelocity; return result; +} +void idPhysics_ParametricMM::SetWaterEntNum(int) {} +int idPhysics_ParametricMM::GetWaterEntNum() { return -1; } +void idPhysics_ParametricMM::SetWaterSurfaceWrldHeight(float) {} +float idPhysics_ParametricMM::GetWaterSurfaceWrldHeight() { return 0.0f; } +void idPhysics_ParametricMM::GetImpactInfo(int, const idVec3*, + impactInfo_t* info) { + if (info != nullptr) { info->Zero(); info->velocity = current.linearVelocity; } +} +void idPhysics_ParametricMM::ApplyImpulse(int, const idVec3*, const idVec3*) {} +void idPhysics_ParametricMM::ApplyForce(int, const idVec3*, const idVec3*) {} +void idPhysics_ParametricMM::Activate() { + current.atRest = -1; + if (callbacks != nullptr) + GameLib_NotifyPhysicsActivated(callbacks, GetPhysicsId()); +} +void idPhysics_ParametricMM::PutToRest() { + current.atRest = current.time; current.linearVelocity.Zero(); + current.angularVelocity.Zero(); +} +bool idPhysics_ParametricMM::IsAtRest() { return current.atRest >= 0; } +bool idPhysics_ParametricMM::IsPushable(int) { return false; } +void idPhysics_ParametricMM::SaveState() { saved = current; } +void idPhysics_ParametricMM::RestoreState() { current = saved; LinkClip(); } +void idPhysics_ParametricMM::SetLinearExtrapolation(extrapolation_t type, + int currentTime, int startTime, int duration, const idVec3& base, + const idVec3& baseSpeed, const idVec3& speed) { + current.time = currentTime; + current.linearExtrapolation.Init(static_cast(startTime), + static_cast(duration), base, baseSpeed, speed, type); + current.localOrigin = base; Activate(); +} +void idPhysics_ParametricMM::SetAngularExtrapolation(extrapolation_t type, + int currentTime, int startTime, int duration, + const idMat3& startAxis, const idVec3& rotationVector, + float baseSpeed, float speed) { + current.time = currentTime; + current.angularExtrapolationStartAxis = startAxis; + current.angularExtrapolationRotVec = rotationVector; + current.angularExtrapolation.Init(static_cast(startTime), + static_cast(duration), 0.0f, baseSpeed, speed, type); + Activate(); +} +void idPhysics_ParametricMM::SetSpline(idCurve_Spline* spline, + int accelTime, int decelTime, bool useAngles) { + current.spline = spline; current.useSplineAngles = useAngles; + if (spline != nullptr && spline->GetNumValues() > 0) { + const float start = spline->GetTime(0); + const float end = spline->GetTime(spline->GetNumValues() - 1); + current.totalLength = spline->GetLengthForTime(end); + current.splineInterpolate.InitDuration(start, 0.0f, + end - start, 0.0f, current.totalLength); + (void)accelTime; (void)decelTime; + } + Activate(); +} + +bool idPhysics_ParametricMM::Evaluate(int timeStepMSec, int endTimeMSec) { + if (current.pauseTime > endTimeMSec) return false; + const idVec3 oldOrigin = current.worldOrigin; + const idMat3 oldAxis = current.worldAxis; + current.time = endTimeMSec; + idVec3 localOrigin = current.linearExtrapolation.GetCurrentValue( + static_cast(endTimeMSec)); + if (current.linearInterpolation.GetDuration() > 0.0f) + localOrigin = current.linearInterpolation.GetCurrentValue( + static_cast(endTimeMSec)); + if (current.spline != nullptr && current.spline->GetNumValues() > 0) { + current.length = current.splineInterpolate.GetCurrentValue( + static_cast(endTimeMSec)); + const float time = current.spline->GetTimeForLength(current.length); + localOrigin = current.spline->GetCurrentValue(time); + current.splineDerivative = current.spline->GetCurrentFirstDerivative(time); + } + idMat3 localAxis = current.localAxis; + if (current.angularInterpolation.GetDuration() > 0.0f) + localAxis = QuatToMat3(current.angularInterpolation.GetCurrentValue( + static_cast(endTimeMSec))); + else { + const float angle = current.angularExtrapolation.GetCurrentValue( + static_cast(endTimeMSec)); + idRotation rotation(kParametricZeroVector, + current.angularExtrapolationRotVec, angle); + localAxis = current.angularExtrapolationStartAxis * rotation.ToMat3(); + } + current.localOrigin = localOrigin; + current.localAxis = localAxis; + idVec3 desiredOrigin = localOrigin; + idMat3 desiredAxis = localAxis; + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) { + desiredOrigin = masterOrigin + masterAxis * localOrigin; + if (isOrientated) desiredAxis *= masterAxis; + } + } + if (hasWorldOrientation) { + desiredOrigin = worldOrigin + worldAxis * desiredOrigin; + desiredAxis *= worldAxis; + } + blockingPhysicsId = -1; + if (isPusher && pusher != nullptr) { + trace_t trace{}; + pusher->ClipPush(trace, this, pushFlags, oldOrigin, oldAxis, + desiredOrigin, desiredAxis); + if (trace.fraction < 1.0f) blockingPhysicsId = trace.c.physicsId; + } + current.worldOrigin = desiredOrigin; + current.worldAxis = desiredAxis; + if (timeStepMSec > 0) { + current.linearVelocity = (desiredOrigin - oldOrigin) + * (1000.0f / static_cast(timeStepMSec)); + const idAngles oldAngles = Mat3ToAngles(oldAxis); + const idAngles newAngles = Mat3ToAngles(desiredAxis); + current.angularVelocity = idVec3( + newAngles.pitch - oldAngles.pitch, + newAngles.yaw - oldAngles.yaw, + newAngles.roll - oldAngles.roll) + * (1000.0f / static_cast(timeStepMSec)); + } + LinkClip(); + return (desiredOrigin - oldOrigin).LengthSqr() != 0.0f + || MatrixChanged(desiredAxis, oldAxis); +} +void idPhysics_ParametricMM::UpdateTime(int endTimeMSec) { + const float delta = static_cast(endTimeMSec - current.time); + current.time = endTimeMSec; + current.linearExtrapolation.SetStartTime( + current.linearExtrapolation.GetStartTime() + delta); + current.angularExtrapolation.SetStartTime( + current.angularExtrapolation.GetStartTime() + delta); + if (current.spline != nullptr) current.spline->ShiftTime(delta); +} +void idPhysics_ParametricMM::ClipRotation(trace_t* results, + const idRotation* rotation, const idClipModel* model) { + if (results == nullptr) return; + const idClipModel* moving = model != nullptr ? model : clipModel; + if (clip == nullptr || moving == nullptr || rotation == nullptr) { + std::memset(results, 0, sizeof(*results)); results->fraction = 1.0f; + return; + } + clip->Rotation(results, current.worldOrigin, *rotation, moving, + current.worldAxis, clipMask, GetEntityNumber(), false, + "idPhysics_ParametricMM::ClipRotation"); +} +int idPhysics_ParametricMM::ClipContents(const idClipModel* model, + int mask) { + if (clip == nullptr || clipModel == nullptr) return 0; + if (mask == 0) mask = clipMask; + trace_t result{}; + if (model != nullptr) + clip->ContentsModel(result, current.worldOrigin, clipModel, + current.worldAxis, mask, model->GetOrigin(), model, + model->GetAxis()); + else + clip->Contents(&result, current.worldOrigin, clipModel, + current.worldAxis, mask, GetEntityNumber(), + "idPhysics_ParametricMM::ClipContents"); + return result.c.contentFlags; +} +void idPhysics_ParametricMM::DisableClip() { if (clipModel) clipModel->Disable(); } +void idPhysics_ParametricMM::EnableClip() { if (clipModel) clipModel->Enable(); } +void idPhysics_ParametricMM::UnlinkClip() { if (clipModel) clipModel->Unlink(); } +void idPhysics_ParametricMM::LinkClip() { + if (clipModel) clipModel->Link(GetEntityNumber(), GetEntityNumber(), 0, + current.worldOrigin, current.worldAxis); +} +bool idPhysics_ParametricMM::EvaluateContacts() { return false; } +void idPhysics_ParametricMM::SetPushed(int) {} +idVec3* idPhysics_ParametricMM::GetPushedLinearVelocity(idVec3* result, int) { + if (result) result->Zero(); return result; +} +idVec3* idPhysics_ParametricMM::GetPushedAngularVelocity(idVec3* result, int) { + if (result) result->Zero(); return result; +} +void idPhysics_ParametricMM::SetMaster(bool enable, + const idVec3* masterOrigin, const idMat3* masterAxis, + bindFlags_t flags) { + if (enable && masterOrigin != nullptr && masterAxis != nullptr) { + current.localOrigin = masterAxis->Transpose() + * (current.worldOrigin - *masterOrigin); + current.localAxis = (static_cast(flags) & 1) != 0 + ? current.worldAxis * masterAxis->Transpose() : current.worldAxis; + hasMaster = true; isOrientated = (static_cast(flags) & 1) != 0; + } else hasMaster = false; +} +void idPhysics_ParametricMM::SetLocalOrigin(const idVec3* value, int) { + if (value) { current.localOrigin = *value; Activate(); } +} +void idPhysics_ParametricMM::SetLocalAxis(const idMat3* value, int) { + if (value) { current.localAxis = *value; Activate(); } +} +int idPhysics_ParametricMM::GetBlockingEntityNum() { + idPhysics* physics = idPhysics::GetPhysicsForId(blockingPhysicsId); + return physics ? physics->GetEntityNumber() : 0x1FFF; +} +int idPhysics_ParametricMM::GetLinearEndTime() { + return (static_cast(current.linearExtrapolation.extrapolationType) + & EXTRAPOLATION_NOSTOP) != 0 ? 0 + : static_cast(current.linearExtrapolation.GetEndTime()); +} +int idPhysics_ParametricMM::GetAngularEndTime() { + return (static_cast(current.angularExtrapolation.extrapolationType) + & EXTRAPOLATION_NOSTOP) != 0 ? 0 + : static_cast(current.angularExtrapolation.GetEndTime()); +} +bool idPhysics_ParametricMM::IsOutsideWorld() { + return idPhysics_DynamicBase::IsOutsideWorld(); +} diff --git a/source/engine/gamelib/physics/physics_parametric.h b/source/engine/gamelib/physics/physics_parametric.h index e4f1485..2221a35 100644 --- a/source/engine/gamelib/physics/physics_parametric.h +++ b/source/engine/gamelib/physics/physics_parametric.h @@ -1,204 +1,268 @@ #pragma once -// Reconstructed C++ declarations from IDA Local Types and PDB/DIA metadata. -// Original PDB header: w:\tech5\engine\gamelib\physics\physics_parametric.h -// Recovered logical types: 2 -// Signatures retain Xbox 360 ABI evidence and may still require manual review. +#include "gamelib/physics/physics_dynamicbase.h" +#include "idlib/containers/list.h" +#include "idlib/math/curve.h" +#include "idlib/math/interpolate.h" +class idPush; -// IDA Local Type ordinal 15500; PDB kind: class. -class idPhysics_Parametric : public idPhysics_DynamicBase -{ -public: - // Recovered virtual interface; IDA vtable ordinal 15501. - virtual ~idPhysics_Parametric(); - virtual void SetClipModel(idClipModel *, float, int, bool); - virtual idClipModel *GetClipModel(int); - virtual int GetNumClipModels(); - virtual void SetMass(float, int); - virtual float GetMass(int); - virtual void SetContents(int, int); - virtual int GetContents(int); - virtual void SetClipMask(int, int); - virtual int GetClipMask(int); - virtual const idBounds *GetBounds(int); - virtual const idBounds *GetAbsBounds(int); - virtual void SetOrigin(const idVec3 *, int); - virtual void SetAxis(const idMat3 *, int); - virtual void Translate(const idVec3 *, int); - virtual void Rotate(const idRotation *, int); - virtual const idVec3 *GetOrigin(int); - virtual const idMat3 *GetAxis(int); - virtual const idVec3 *GetLocalOrigin(int); - virtual const idMat3 *GetLocalAxis(int); - virtual void SetLinearVelocity(const idVec3 *, int); - virtual void SetAngularVelocity(const idVec3 *, int); - virtual idVec3 *GetLinearVelocity(idVec3 *result, int); - virtual idVec3 *GetAngularVelocity(idVec3 *result, int); - virtual void SetGravity(const idVec3 *); - virtual const idVec3 *GetGravity(); - virtual const idVec3 *GetGravityNormal(); - virtual void SetWaterLevel(float, int); - virtual float GetWaterLevel(int); - virtual void SetWaterViscosity(float, int); - virtual float GetWaterViscosity(int); - virtual void SetWaterEntNum(int); - virtual int GetWaterEntNum(); - virtual void SetWaterSurfaceWrldHeight(float); - virtual float GetWaterSurfaceWrldHeight(); - virtual void GetImpactInfo(const int, const idVec3 *, impactInfo_t *); - virtual void ApplyImpulse(const int, const idVec3 *, const idVec3 *); - virtual void ApplyForce(const int, const idVec3 *, const idVec3 *); - virtual void Activate(); - virtual void PutToRest(); - virtual bool IsAtRest(); - virtual bool IsPushable(int); - virtual void SaveState(); - virtual void RestoreState(); - virtual bool Evaluate(int, int); - virtual void UpdateTime(int); - virtual void ClipTranslation(trace_t *, const idVec3 *, const idClipModel *); - virtual void ClipRotation(trace_t *, const idRotation *, const idClipModel *); - virtual int ClipContents(const idClipModel *, int); - virtual void DisableClip(); - virtual void EnableClip(); - virtual void UnlinkClip(); - virtual void LinkClip(); - virtual bool EvaluateContacts(); - virtual int GetNumContacts(); - virtual const contactInfo_t *GetContact(int); - virtual void ClearContacts(); - virtual void AddContactPhysics(idPhysics *); - virtual void RemoveContactPhysics(idPhysics *); - virtual int GetNumContactPhysics(); - virtual idPhysics *GetContactPhysics(int); - virtual void ActivateContactPhysics(); - virtual bool HasGroundContacts(); - virtual bool IsGroundEntity(int); - virtual bool IsGroundClipModel(int, int); - virtual void SetPushed(int); - virtual idVec3 *GetPushedLinearVelocity(idVec3 *result, const int); - virtual idVec3 *GetPushedAngularVelocity(idVec3 *result, const int); - virtual void SetMaster(bool, const idVec3 *, const idMat3 *, const bindFlags_t); - virtual void SetLocalOrigin(const idVec3 *, int); - virtual void SetLocalAxis(const idMat3 *, int); - virtual int GetBlockingEntityNum(); - virtual int GetLinearEndTime(); - virtual int GetAngularEndTime(); - virtual bool IsInNonResidentCollisionArea(bool); - virtual bool IsOutsideWorld(); +struct parametricPState_t { + int time; + int atRest; + idVec3 worldOrigin; + idAngles worldAngles; + idMat3 worldAxis; + idVec3 localOrigin; + idAngles localAngles; + idExtrapolate linearExtrapolation; + idExtrapolate angularExtrapolation; + idInterpolateAccelDecelLinear linearInterpolation; + idInterpolateAccelDecelLinear angularInterpolation; + idCurve_Spline* spline; + idCurve_Spline* angularSpline; + idInterpolateAccelDecelLinear splineInterpolate; + bool useSplineAngles; - parametricPState_t current; - parametricPState_t saved; - idVec6 spatialVelocity; - int blockingPhysicsId; - idBounds absBounds; - idPush *pusher; - bool isPusher; - idClipModel *clipModel; - int pushFlags; - bool hasMaster; - bool isOrientated; - bool hasWorldOrientation; - idMat3 worldAxis; - idVec3 worldOrigin; + parametricPState_t(); }; -// IDA Local Type ordinal 19690; PDB kind: class. -class __declspec(align(4)) idPhysics_ParametricMM : public idPhysics_DynamicBase -{ +class idPhysics_Parametric : public idPhysics_DynamicBase { public: - // Recovered virtual interface; IDA vtable ordinal 19694. - virtual ~idPhysics_ParametricMM(); - virtual void SetClipModel(idClipModel *, float, int, bool); - virtual idClipModel *GetClipModel(int); - virtual int GetNumClipModels(); - virtual void SetMass(float, int); - virtual float GetMass(int); - virtual void SetContents(int, int); - virtual int GetContents(int); - virtual void SetClipMask(int, int); - virtual int GetClipMask(int); - virtual const idBounds *GetBounds(int); - virtual const idBounds *GetAbsBounds(int); - virtual void SetOrigin(const idVec3 *, int); - virtual void SetAxis(const idMat3 *, int); - virtual void Translate(const idVec3 *, int); - virtual void Rotate(const idRotation *, int); - virtual const idVec3 *GetOrigin(int); - virtual const idMat3 *GetAxis(int); - virtual const idVec3 *GetLocalOrigin(int); - virtual const idMat3 *GetLocalAxis(int); - virtual void SetLinearVelocity(const idVec3 *, int); - virtual void SetAngularVelocity(const idVec3 *, int); - virtual idVec3 *GetLinearVelocity(idVec3 *result, int); - virtual idVec3 *GetAngularVelocity(idVec3 *result, int); - virtual void SetGravity(const idVec3 *); - virtual const idVec3 *GetGravity(); - virtual const idVec3 *GetGravityNormal(); - virtual void SetWaterLevel(float, int); - virtual float GetWaterLevel(int); - virtual void SetWaterViscosity(float, int); - virtual float GetWaterViscosity(int); - virtual void SetWaterEntNum(int); - virtual int GetWaterEntNum(); - virtual void SetWaterSurfaceWrldHeight(float); - virtual float GetWaterSurfaceWrldHeight(); - virtual void GetImpactInfo(const int, const idVec3 *, impactInfo_t *); - virtual void ApplyImpulse(const int, const idVec3 *, const idVec3 *); - virtual void ApplyForce(const int, const idVec3 *, const idVec3 *); - virtual void Activate(); - virtual void PutToRest(); - virtual bool IsAtRest(); - virtual bool IsPushable(int); - virtual void SaveState(); - virtual void RestoreState(); - virtual bool Evaluate(int, int); - virtual void UpdateTime(int); - virtual void ClipTranslation(trace_t *, const idVec3 *, const idClipModel *); - virtual void ClipRotation(trace_t *, const idRotation *, const idClipModel *); - virtual int ClipContents(const idClipModel *, int); - virtual void DisableClip(); - virtual void EnableClip(); - virtual void UnlinkClip(); - virtual void LinkClip(); - virtual bool EvaluateContacts(); - virtual int GetNumContacts(); - virtual const contactInfo_t *GetContact(int); - virtual void ClearContacts(); - virtual void AddContactPhysics(idPhysics *); - virtual void RemoveContactPhysics(idPhysics *); - virtual int GetNumContactPhysics(); - virtual idPhysics *GetContactPhysics(int); - virtual void ActivateContactPhysics(); - virtual bool HasGroundContacts(); - virtual bool IsGroundEntity(int); - virtual bool IsGroundClipModel(int, int); - virtual void SetPushed(int); - virtual idVec3 *GetPushedLinearVelocity(idVec3 *result, const int); - virtual idVec3 *GetPushedAngularVelocity(idVec3 *result, const int); - virtual void SetMaster(bool, const idVec3 *, const idMat3 *, const bindFlags_t); - virtual void SetLocalOrigin(const idVec3 *, int); - virtual void SetLocalAxis(const idMat3 *, int); - virtual int GetBlockingEntityNum(); - virtual int GetLinearEndTime(); - virtual int GetAngularEndTime(); - virtual bool IsInNonResidentCollisionArea(bool); - virtual bool IsOutsideWorld(); + idPhysics_Parametric(); + ~idPhysics_Parametric() override; - parametricPState_tMM current; - parametricPState_tMM saved; - int blockingPhysicsId; - idBounds absBounds; - idPush *pusher; - bool isPusher; - idClipModel *clipModel; - int pushFlags; - bool hasMaster; - bool isOrientated; - bool hasWorldOrientation; - idMat3 worldAxis; - idVec3 worldOrigin; - int collideClipMask; - bool collideCallbackEnabled; + void SetClipModel(idClipModel*, float, int, bool) override; + idClipModel* GetClipModel(int) override; + int GetNumClipModels() override; + void SetMass(float, int) override; + float GetMass(int) override; + void SetContents(int, int) override; + int GetContents(int) override; + const idBounds* GetBounds(int) override; + const idBounds* GetAbsBounds(int) override; + void SetOrigin(const idVec3*, int) override; + void SetAxis(const idMat3*, int) override; + void Translate(const idVec3*, int) override; + void Rotate(const idRotation*, int) override; + const idVec3* GetOrigin(int) override; + const idMat3* GetAxis(int) override; + const idVec3* GetLocalOrigin(int) override; + const idMat3* GetLocalAxis(int) override; + void SetLinearVelocity(const idVec3*, int) override; + void SetAngularVelocity(const idVec3*, int) override; + idVec3* GetLinearVelocity(idVec3*, int) override; + idVec3* GetAngularVelocity(idVec3*, int) override; + void SetWaterEntNum(int) override; + int GetWaterEntNum() override; + void SetWaterSurfaceWrldHeight(float) override; + float GetWaterSurfaceWrldHeight() override; + void GetImpactInfo(int, const idVec3*, impactInfo_t*) override; + void ApplyImpulse(int, const idVec3*, const idVec3*) override; + void ApplyForce(int, const idVec3*, const idVec3*) override; + void Activate() override; + void PutToRest() override; + bool IsAtRest() override; + bool IsPushable(int) override; + void SaveState() override; + void RestoreState() override; + bool Evaluate(int, int) override; + void UpdateTime(int) override; + void ClipRotation(trace_t*, const idRotation*, const idClipModel*) override; + int ClipContents(const idClipModel*, int) override; + void DisableClip() override; + void EnableClip() override; + void UnlinkClip() override; + void LinkClip() override; + bool EvaluateContacts() override; + void SetPushed(int) override; + idVec3* GetPushedLinearVelocity(idVec3*, int) override; + idVec3* GetPushedAngularVelocity(idVec3*, int) override; + void SetMaster(bool, const idVec3*, const idMat3*, bindFlags_t) override; + void SetLocalOrigin(const idVec3*, int) override; + void SetLocalAxis(const idMat3*, int) override; + int GetBlockingEntityNum() override; + int GetLinearEndTime() override; + int GetAngularEndTime() override; + + bool IsOutsideWorld() override; + + void SetPusher(idPush* push, int flags); + bool IsPusher() const; + idCurve_Spline* GetSpline(); + const idAngles* GetLocalAngles() const; + void SetLinearExtrapolation(extrapolation_t type, int currentTime, + int startTime, int duration, const idVec3& base, + const idVec3& baseSpeed, const idVec3& speed); + void SetAngularExtrapolation(extrapolation_t type, int currentTime, + int startTime, int duration, const idAngles& base, + const idAngles& baseSpeed, const idAngles& speed); + idAngles GetCurrentAngularExtrapolationAngles(int time) const; + void SetLinearInterpolation(int currentTime, int startTime, + int accelTime, int decelTime, int duration, + const idVec3& start, const idVec3& end); + void SetAngularInterpolation(int currentTime, int startTime, + int accelTime, int decelTime, int duration, + const idAngles& start, const idAngles& end); + void SetSpline(idCurve_Spline* spline, int accelTime, + int decelTime, bool useSplineAngles, + idCurve_Spline* angularSpline = nullptr); + void SetWorldOrientation(const idVec3& origin, const idMat3& axis); + void ClearWorldOrientation(); + void ForceUpdateSpatialVelocity(int timeStepMSec); + extrapolation_t GetAngularExtrapolationType() const; + + parametricPState_t current; + parametricPState_t saved; + idVec6 spatialVelocity; + int blockingPhysicsId; + idBounds absBounds; + idPush* pusher; + bool isPusher; + idClipModel* clipModel; + int pushFlags; + bool hasMaster; + bool isOrientated; + bool hasWorldOrientation; + idMat3 worldAxis; + idVec3 worldOrigin; }; + +class idPhysics_ParametricMM; +struct parametricSplineBatchInfo_tMM { + float DistanceFromMe; + idPhysics_ParametricMM* PhysObj; +}; + +struct parametricPState_tMM { + int time; + float length; + float totalLength; + int atRest; + idVec3 worldOrigin; + idMat3 worldAxis; + idVec3 localOrigin; + idMat3 localAxis; + idExtrapolate linearExtrapolation; + idExtrapolate angularExtrapolation; + idMat3 angularExtrapolationStartAxis; + idVec3 angularExtrapolationRotVec; + idVec3 linearVelocity; + idVec3 angularVelocity; + idInterpolateAccelDecelLinear linearInterpolation; + idInterpolateAccelDecelLinear angularInterpolation; + idInterpolateAccelLinearEx splineInterpolate; + idInterpolate splineAngleInterpolate; + idCurve_Spline* spline; + idVec3 splineDerivative; + bool useSplineAngles; + int pauseTime; + idExtrapolate oscillationExtrapolation[3]; + bool oscillationZRelative; + idInterpolate localOffsetInterpolation; + idPhysics_ParametricMM* splineMaster; + idInterpolate localOriginScaleInterpolation; + idList splineChilds; + bool splineDummy; + + parametricPState_tMM(); +}; + +class idPhysics_ParametricMM : public idPhysics_DynamicBase { +public: + idPhysics_ParametricMM(); + ~idPhysics_ParametricMM() override; + void SetClipModel(idClipModel*, float, int, bool) override; + idClipModel* GetClipModel(int) override; + int GetNumClipModels() override; + void SetMass(float, int) override; + float GetMass(int) override; + void SetContents(int, int) override; + int GetContents(int) override; + const idBounds* GetBounds(int) override; + const idBounds* GetAbsBounds(int) override; + void SetOrigin(const idVec3*, int) override; + void SetAxis(const idMat3*, int) override; + void Translate(const idVec3*, int) override; + void Rotate(const idRotation*, int) override; + const idVec3* GetOrigin(int) override; + const idMat3* GetAxis(int) override; + const idVec3* GetLocalOrigin(int) override; + const idMat3* GetLocalAxis(int) override; + void SetLinearVelocity(const idVec3*, int) override; + void SetAngularVelocity(const idVec3*, int) override; + idVec3* GetLinearVelocity(idVec3*, int) override; + idVec3* GetAngularVelocity(idVec3*, int) override; + void SetWaterEntNum(int) override; + int GetWaterEntNum() override; + void SetWaterSurfaceWrldHeight(float) override; + float GetWaterSurfaceWrldHeight() override; + void GetImpactInfo(int, const idVec3*, impactInfo_t*) override; + void ApplyImpulse(int, const idVec3*, const idVec3*) override; + void ApplyForce(int, const idVec3*, const idVec3*) override; + void Activate() override; + void PutToRest() override; + bool IsAtRest() override; + bool IsPushable(int) override; + void SaveState() override; + void RestoreState() override; + bool Evaluate(int, int) override; + void UpdateTime(int) override; + void ClipRotation(trace_t*, const idRotation*, const idClipModel*) override; + int ClipContents(const idClipModel*, int) override; + void DisableClip() override; + void EnableClip() override; + void UnlinkClip() override; + void LinkClip() override; + bool EvaluateContacts() override; + void SetPushed(int) override; + idVec3* GetPushedLinearVelocity(idVec3*, int) override; + idVec3* GetPushedAngularVelocity(idVec3*, int) override; + void SetMaster(bool, const idVec3*, const idMat3*, bindFlags_t) override; + void SetLocalOrigin(const idVec3*, int) override; + void SetLocalAxis(const idMat3*, int) override; + int GetBlockingEntityNum() override; + int GetLinearEndTime() override; + int GetAngularEndTime() override; + bool IsOutsideWorld() override; + + void SetPusher(idPush* push, int flags); + idCurve_Spline* GetSpline(); + void SetUseSplineAngles(bool value); + void PauseParametricUntil(int resumeTime); + void SetLinearExtrapolation(extrapolation_t type, int currentTime, + int startTime, int duration, const idVec3& base, + const idVec3& baseSpeed, const idVec3& speed); + void SetAngularExtrapolation(extrapolation_t type, int currentTime, + int startTime, int duration, const idMat3& startAxis, + const idVec3& rotationVector, float baseSpeed, float speed); + void SetSpline(idCurve_Spline* spline, int accelTime, + int decelTime, bool useSplineAngles); + + parametricPState_tMM current; + parametricPState_tMM saved; + int blockingPhysicsId; + idBounds absBounds; + idPush* pusher; + bool isPusher; + idClipModel* clipModel; + int pushFlags; + bool hasMaster; + bool isOrientated; + bool hasWorldOrientation; + idMat3 worldAxis; + idVec3 worldOrigin; + int collideClipMask; + bool collideCallbackEnabled; +}; + +#if defined(_WIN32) && !defined(_WIN64) +static_assert(sizeof(parametricPState_t) == 496, + "Recovered parametric state ABI changed"); +static_assert(sizeof(idPhysics_Parametric) == 1232, + "Recovered idPhysics_Parametric ABI changed"); +static_assert(sizeof(parametricPState_tMM) == 872, + "Recovered MM parametric state ABI changed"); +static_assert(sizeof(idPhysics_ParametricMM) == 1968, + "Recovered idPhysics_ParametricMM ABI changed"); +#endif diff --git a/source/engine/gamelib/physics/physics_player.cpp b/source/engine/gamelib/physics/physics_player.cpp new file mode 100644 index 0000000..ed8b71c --- /dev/null +++ b/source/engine/gamelib/physics/physics_player.cpp @@ -0,0 +1,978 @@ +#include "gamelib/physics/physics_player.h" + +#include "gamelib/physics/clipmodel.h" +#include "idlib/geometry/tracemodel.h" +#include "idlib/math/rotation.h" + +#include +#include +#include + +bool GameLib_GetMasterPhysicsTransform(idPhysicsCallbacks* callbacks, + idVec3& origin, idMat3& axis); +void GameLib_NotifyPhysicsActivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsDeactivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsCollision(idPhysicsCallbacks* callbacks, + int physicsId, const trace_t& collision, const idVec3& velocity); +void GameLib_SerializePlayerState(idSerializer* serializer, + playerPState_t& state, playerPState_t* catchupStart); +void GameLib_SerializePlayerNonPredictive(idSerializer* serializer, + idPhysics_Player& physics); + +namespace { + +constexpr int ENTITYNUM_NONE = 0x1FFF; +constexpr int PMF_JUMPED = 0x0001; +constexpr int PMF_DUCKED = 0x0002; +constexpr int PMF_STEPPED_UP = 0x0004; +constexpr int PMF_SPRINT_SLIDE = 0x0100; +constexpr float MIN_FLOOR_COSINE = 0.70f; + +const idVec3 kZeroVector(0.0f, 0.0f, 0.0f); +const idMat3 kIdentityAxis(1.0f); + +float AxisYaw(const idMat3& axis) { + return std::atan2(axis[0].y, axis[0].x) * 57.29577951308232f; +} + +void ProjectOntoPlane(idVec3& vector, const idVec3& normal, + const float overBounce = 1.0f) { + const float backoff = vector.Dot(normal) * overBounce; + vector = vector - normal * backoff; +} + +idBounds ScaledHeightBounds(const idBounds& source, const float scale) { + idBounds result = source; + const float height = source[1].z - source[0].z; + result[1].z = source[0].z + height * scale; + return result; +} + +idClipModel* MakePlayerClipModel(idClip* owner, const idBounds& bounds, + const int contents, const idMaterial* material) { + idTraceModel traceModel; + traceModel.SetupCylinder(bounds, 8); + idClipModel* const model = new idClipModel(owner, &traceModel, -1, + material); + model->SetContents(contents); + return model; +} + +} // namespace + +idPhysics_Player::idPhysics_Player() + : idPhysics_Actor() + , explicitMove{} + , explicitMove2{} + , current{} + , previous{} + , saved{} + , clipModel_standing(nullptr) + , clipModel_crouched(nullptr) + , clipModel_sprintSlide(nullptr) + , clipModel_dead(nullptr) + , overrideClipMaterial(nullptr) + , pusher(nullptr) + , slideMoveQuery{} + , ladderQuery1{} + , ladderQuery2{} + , contentsQuery{} + , lastContents(0) + , walkSpeed(0.0f) + , crouchSpeed(0.0f) + , maxStepHeight(0.0f) + , maxJumpHeight(0.0f) + , debugLevel(0) + , walkAccelerate(10.0f) + , airAccelerate(1.0f) + , flyAccelerate(8.0f) + , waterAccelerate(4.0f) + , walkFriction(6.0f) + , airFriction(0.0f) + , flyFriction(3.0f) + , waterFriction(3.0f) + , slideFriction(1.5f) + , command{} + , prevcmd{} + , viewAngles{} + , framemsec(0) + , frametime(0.0f) + , playerSpeed(0.0f) + , viewForward(1.0f, 0.0f, 0.0f) + , viewRight(0.0f, -1.0f, 0.0f) + , walking(false) + , startedOnGround(false) + , groundPlane(false) + , groundTrace{} + , groundSurfaceFlags(0) + , inhibitSprint(false) + , inhibitJump(false) + , toggleCrouch(false) + , ladder(false) + , ladderNormal(0.0f, 0.0f, 0.0f) + , canLadder(true) + , currentSwimDepthState(SWIMDEPTH_STATE_NONE) + , swimStrokeVelocity(0.0f, 0.0f, 0.0f) + , swimStrokeTimeLeft(-1000) + , swimStrokeTime(-1000) + , swimStrokeAltCurve(false) + , swimStrokeIgnoreInput(false) + , currentWaterEnt(-1) + , currentWaterSurfaceHeight(0.0f) + , blockedAccelerationPercent(0.0f) + , blockedAccelerationVec(0.0f, 0.0f, 0.0f) + , modifiedMovementMaxPercent(1.0f) + , modifiedMovementAngle(90.0f) + , modifiedAccelerationVec(0.0f, 0.0f, 0.0f) + , canCrouchSprint(false) + , lastJumpChance(0) + , perfectOriginPosition(0.0f, 0.0f, 0.0f) + , perfectOriginDuration(0) + , perfectOriginTime(0) + , externalVelocity(0.0f, 0.0f, 0.0f) + , clientOriginSet(false) + , nextClientOrigin(0.0f, 0.0f, 0.0f) + , forcedClientCrouchState(false) + , clientPusherLocked(false) + , pushDeltaZ(0.0f) + , ownerJumpHandler(nullptr) + , ownerCrouchHandler(nullptr) + , ownerCrouchJumpHandler(nullptr) { + type = PHYSICS_PLAYER; + current.movementType = PM_NORMAL; + previous.movementType = PM_NORMAL; + saved.movementType = PM_NORMAL; + explicitMove.clipMove = true; + explicitMove.velocityType = playerExplicitMove_t::VEL_EXPLICIT; + explicitMove.ignoreEntityNum = ENTITYNUM_NONE; + explicitMove2.clipMove = true; + explicitMove2.velocityType = playerExplicitMove_t::VEL_EXPLICIT; + explicitMove2.ignoreEntityNum = ENTITYNUM_NONE; + groundTrace.fraction = 1.0f; + groundTrace.endAxis = kIdentityAxis; +} + +idPhysics_Player::~idPhysics_Player() { + UnlinkClip(); + idClipModel* models[] = {clipModel_standing, clipModel_crouched, + clipModel_sprintSlide, clipModel_dead}; + for (int i = 0; i < 4; ++i) { + if (models[i] == nullptr) continue; + bool duplicate = false; + for (int j = 0; j < i; ++j) duplicate |= models[j] == models[i]; + if (!duplicate) models[i]->Delete(); + } + clipModels[0] = nullptr; + clipModels[1] = nullptr; +} + +void idPhysics_Player::SelectClipModel(idClipModel* const model) { + if (model == nullptr || model == clipModels[0]) return; + if (clipModels[0] != nullptr) clipModels[0]->Unlink(); + clipModels[0] = model; + idPhysics_Actor::LinkClip(current.worldOrigin, clipModelAxis); +} + +void idPhysics_Player::SetClipModel(idClipModel* const model, + const float density, const int id, const bool freeOld) { + if (id != 0 || model == nullptr) { + idPhysics_Actor::SetClipModel(model, density, id, freeOld); + return; + } + const idBounds sourceBounds = model->GetBounds(); + const int contents = model->GetContents(); + const idVec3 oldOrigin = model->GetOrigin(); + if (freeOld) model->Delete(); + + idClipModel* oldModels[] = {clipModel_standing, clipModel_crouched, + clipModel_sprintSlide, clipModel_dead}; + for (int i = 0; i < 4; ++i) { + if (oldModels[i] == nullptr || oldModels[i] == model) continue; + bool duplicate = false; + for (int j = 0; j < i; ++j) duplicate |= oldModels[j] == oldModels[i]; + if (!duplicate) oldModels[i]->Delete(); + } + + clipModel_standing = MakePlayerClipModel(clip, sourceBounds, contents, + overrideClipMaterial); + clipModel_crouched = MakePlayerClipModel(clip, + ScaledHeightBounds(sourceBounds, 0.65f), contents, + overrideClipMaterial); + clipModel_sprintSlide = MakePlayerClipModel(clip, + ScaledHeightBounds(sourceBounds, 0.45f), contents, + overrideClipMaterial); + clipModel_dead = MakePlayerClipModel(clip, + ScaledHeightBounds(sourceBounds, 0.35f), contents, + overrideClipMaterial); + clipModels[0] = nullptr; + SelectClipModel(clipModel_standing); + current.worldOrigin = oldOrigin; + current.localOrigin = oldOrigin; + LinkClip(current.worldOrigin, clipModelAxis); + if (density > 0.0f) SetMass(density, 0); +} + +void idPhysics_Player::SetClipModelStanding(idClipModel* const model) { + if (model == nullptr) return; + if (clipModel_standing != nullptr && clipModel_standing != model) + clipModel_standing->Delete(); + clipModel_standing = model; + if (!IsCrouching()) SelectClipModel(model); +} + +void idPhysics_Player::SetOrigin(const idVec3* const origin, int) { + if (origin == nullptr) return; + slideMoveQuery.index = 0; + current.localOrigin = *origin; + current.worldOrigin = *origin; + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current.worldOrigin = masterOrigin + masterAxis * *origin; + } + LinkClip(current.worldOrigin, clipModelAxis); + Activate(); +} + +void idPhysics_Player::SetAxis(const idMat3* const axis, int) { + if (axis == nullptr) return; + clipModelAxis = *axis; + LinkClip(current.worldOrigin, clipModelAxis); +} + +void idPhysics_Player::Translate(const idVec3* const translation, int) { + if (translation == nullptr) return; + current.localOrigin = current.localOrigin + *translation; + current.worldOrigin = current.worldOrigin + *translation; + LinkClip(current.worldOrigin, clipModelAxis); + Activate(); +} + +void idPhysics_Player::Rotate(const idRotation* const rotation, int) { + if (rotation == nullptr) return; + current.worldOrigin = *rotation * current.worldOrigin; + current.localOrigin = *rotation * current.localOrigin; + clipModelAxis *= rotation->ToMat3(); + LinkClip(current.worldOrigin, clipModelAxis); + Activate(); +} + +const idVec3* idPhysics_Player::GetLocalOrigin(int) { + return ¤t.localOrigin; +} + +const idMat3* idPhysics_Player::GetLocalAxis(int) { + return &clipModelAxis; +} + +void idPhysics_Player::SetLinearVelocity(const idVec3* const velocity, int) { + if (velocity == nullptr) return; + current.velocity = *velocity; + Activate(); +} + +void idPhysics_Player::SetAngularVelocity(const idVec3*, int) {} + +idVec3* idPhysics_Player::GetLinearVelocity(idVec3* const result, int) { + if (result != nullptr) *result = current.velocity; + return result; +} + +idVec3* idPhysics_Player::GetAngularVelocity(idVec3* const result, int) { + if (result != nullptr) result->Zero(); + return result; +} + +void idPhysics_Player::SetWaterLevel(const float level, const int id) { + idPhysics_DynamicBase::SetWaterLevel(level, id); + if (waterLevel <= 0.0f) currentSwimDepthState = SWIMDEPTH_STATE_NONE; + else if (waterLevel < 1.0f) + currentSwimDepthState = SWIMDEPTH_STATE_ON_SURFACE; + else currentSwimDepthState = SWIMDEPTH_STATE_UNDER_SURFACE; +} + +void idPhysics_Player::SetWaterEntNum(const int entityNumber_) { + currentWaterEnt = entityNumber_; +} +int idPhysics_Player::GetWaterEntNum() { return currentWaterEnt; } +void idPhysics_Player::SetWaterSurfaceWrldHeight(const float height) { + currentWaterSurfaceHeight = height; +} +float idPhysics_Player::GetWaterSurfaceWrldHeight() { + return currentWaterSurfaceHeight; +} + +void idPhysics_Player::GetImpactInfo(int, const idVec3*, + impactInfo_t* const info) { + if (info == nullptr) return; + info->Zero(); + info->invMass = invMass; + info->velocity = current.velocity + current.pushVelocity; + info->position = current.worldOrigin; +} + +void idPhysics_Player::ApplyImpulse(int, const idVec3*, + const idVec3* const impulse) { + if (impulse == nullptr) return; + current.velocity = current.velocity + *impulse * invMass; + Activate(); +} + +void idPhysics_Player::ApplyForce(int, const idVec3*, + const idVec3* const force) { + if (force == nullptr) return; + const float time = frametime > 0.0f ? frametime : 0.016f; + current.velocity = current.velocity + *force * (invMass * time); + Activate(); +} + +void idPhysics_Player::Activate() { + if (callbacks != nullptr) + GameLib_NotifyPhysicsActivated(callbacks, GetPhysicsId()); +} +void idPhysics_Player::PutToRest() { current.velocity.Zero(); } +bool idPhysics_Player::IsAtRest() { return false; } +bool idPhysics_Player::IsPushable(int) { return true; } + +void idPhysics_Player::SaveState() { saved = current; } +void idPhysics_Player::RestoreState() { RestoreStateFromState(&saved); } +void idPhysics_Player::RestoreStateFromState(playerPState_t* const state) { + if (state == nullptr) return; + current = *state; + LinkClip(current.worldOrigin, clipModelAxis); +} + +void idPhysics_Player::Serialize(idSerializer* const serializer, + playerPState_t* const catchupStart) { + if (serializer != nullptr) + GameLib_SerializePlayerState(serializer, current, catchupStart); +} + +void idPhysics_Player::SerializeNonPredictiveState( + idSerializer* const serializer) { + if (serializer != nullptr) + GameLib_SerializePlayerNonPredictive(serializer, *this); +} + +void idPhysics_Player::UpdateTime(int) {} + +void idPhysics_Player::SetPushed(const int deltaTime) { + if (deltaTime <= 0) { + current.pushVelocity.Zero(); + return; + } + current.pushVelocity = (current.worldOrigin - saved.worldOrigin) + * (1000.0f / static_cast(deltaTime)); + pushDeltaZ = current.worldOrigin.z - saved.worldOrigin.z; +} + +void idPhysics_Player::ClearPushedVelocity() { + current.pushVelocity.Zero(); + pushDeltaZ = 0.0f; +} + +idVec3* idPhysics_Player::GetPushedLinearVelocity(idVec3* const result, + int) { + if (result != nullptr) *result = current.pushVelocity; + return result; +} + +idVec3* idPhysics_Player::GetPushedAngularVelocity(idVec3* const result, + int) { + if (result != nullptr) result->Zero(); + return result; +} + +void idPhysics_Player::SetMaster(const bool enable, + const idVec3* const masterOrigin, const idMat3* const masterAxis, + bindFlags_t) { + slideMoveQuery.index = 0; + if (enable && masterOrigin != nullptr && masterAxis != nullptr) { + if (!hasMaster) { + current.localOrigin = masterAxis->Transpose() + * (current.worldOrigin - *masterOrigin); + masterYaw = AxisYaw(*masterAxis); + } + hasMaster = true; + ClearContacts(); + } else if (hasMaster) { + hasMaster = false; + Activate(); + } +} + +void idPhysics_Player::SetLocalOrigin(const idVec3* const origin, int) { + if (origin == nullptr) return; + current.localOrigin = *origin; + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current.worldOrigin = masterOrigin + masterAxis * *origin; + } else current.worldOrigin = *origin; + LinkClip(current.worldOrigin, clipModelAxis); +} + +void idPhysics_Player::SetLocalAxis(const idMat3* const axis, int) { + if (axis == nullptr) return; + clipModelAxis = *axis; + LinkClip(current.worldOrigin, clipModelAxis); +} + +int idPhysics_Player::GetBlockingEntityNum() { + return groundTrace.fraction < 1.0f ? groundTrace.c.entityNum + : ENTITYNUM_NONE; +} +int idPhysics_Player::GetLinearEndTime() { return 0; } +int idPhysics_Player::GetAngularEndTime() { return 0; } + +void idPhysics_Player::SetSlideFriction(const float value) { + slideFriction = (std::max)(0.0f, value); +} +bool idPhysics_Player::HasJumped() const { + return (current.movementFlags & PMF_JUMPED) != 0; +} +bool idPhysics_Player::HasCrouched() const { + return (previous.movementFlags & PMF_DUCKED) == 0 && IsCrouching(); +} +bool idPhysics_Player::HasStoodUp() const { + return (previous.movementFlags & PMF_DUCKED) != 0 && !IsCrouching(); +} +bool idPhysics_Player::HasSteppedUp() const { + return (current.movementFlags & PMF_STEPPED_UP) != 0; +} +float idPhysics_Player::GetStepUp() const { return current.stepUp; } +bool idPhysics_Player::IsCrouching() const { + return (current.movementFlags & PMF_DUCKED) != 0; +} +void idPhysics_Player::SetClientCrouch(const bool crouch) { + forcedClientCrouchState = crouch; + SetCrouch(crouch); +} +void idPhysics_Player::SetSprintSliding(const bool sliding) { + if (sliding) current.movementFlags |= PMF_SPRINT_SLIDE; + else current.movementFlags &= ~PMF_SPRINT_SLIDE; + CheckDuck(); +} +void idPhysics_Player::SetWalkFriction(const float friction) { + walkFriction = (std::max)(0.0f, friction); +} +void idPhysics_Player::SetSpeed(const float walkingSpeed, + const float crouchingSpeed) { + walkSpeed = (std::max)(0.0f, walkingSpeed); + crouchSpeed = (std::max)(0.0f, crouchingSpeed); +} +void idPhysics_Player::SetMaxStepHeight(const float value) { + maxStepHeight = (std::max)(0.0f, value); +} +void idPhysics_Player::SetMaxJumpHeight(const float value) { + maxJumpHeight = (std::max)(0.0f, value); +} +void idPhysics_Player::SetMovementType(const pmtype_t movementType) { + current.movementType = movementType >= PM_NORMAL && movementType < PM_MAX + ? movementType : PM_NORMAL; +} +void idPhysics_Player::SetKnockBack(const int knockBackTime, + const bool absoluteKnockBack) { + current.movementTime = absoluteKnockBack + ? knockBackTime : (std::max)(current.movementTime, knockBackTime); +} +void idPhysics_Player::SetDebugLevel(const bool set) { debugLevel = set; } + +void idPhysics_Player::SetSwimStroke(const idVec3& velocity, + const int duration, const bool alternateCurve, + const bool ignoreInput) { + swimStrokeVelocity = velocity; + swimStrokeTimeLeft = duration; + swimStrokeTime = duration; + swimStrokeAltCurve = alternateCurve; + swimStrokeIgnoreInput = ignoreInput; +} + +bool idPhysics_Player::GetIsUnderWater() const { + return currentSwimDepthState == SWIMDEPTH_STATE_UNDER_SURFACE; +} + +void idPhysics_Player::StartPerfectOrigin(const idVec3& idealPosition, + const int lerpTimeMS) { + perfectOriginPosition = idealPosition; + perfectOriginDuration = (std::max)(0, lerpTimeMS); + perfectOriginTime = 0; + current.movementType = PM_PERFECTORIGIN; +} + +void idPhysics_Player::SetClientDeferredOrigin(const idVec3& origin) { + nextClientOrigin = origin; + clientOriginSet = true; +} + +void idPhysics_Player::SetPlayerInput(const usercmd_t& cmd, + const idAngles& newViewAngles) { + prevcmd = command; + command = cmd; + viewAngles = newViewAngles; +} + +bool idPhysics_Player::ClientPusherLocked(bool* const justUnlocked) { + const bool wasLocked = clientPusherLocked; + if (justUnlocked != nullptr) *justUnlocked = false; + if (clientOriginSet) { + clientPusherLocked = false; + if (justUnlocked != nullptr) *justUnlocked = wasLocked; + } + return clientPusherLocked; +} + +void idPhysics_Player::UpdateNonPredictiveClip() { + if (!clientOriginSet) return; + current.worldOrigin = nextClientOrigin; + current.localOrigin = nextClientOrigin; + LinkClip(current.worldOrigin, clipModelAxis); + clientOriginSet = false; +} + +float idPhysics_Player::CmdScale(const usercmd_t& cmd) const { + const int forward = std::abs(static_cast(cmd.forwardmove)); + const int right = std::abs(static_cast(cmd.rightmove)); + const int up = std::abs(static_cast(cmd.upmove)); + const int maximum = (std::max)(forward, (std::max)(right, up)); + if (maximum == 0) return 0.0f; + const float total = std::sqrt(static_cast(forward * forward + + right * right + up * up)); + return total > 0.0f ? playerSpeed * maximum / (127.0f * total) : 0.0f; +} + +void idPhysics_Player::Accelerate(const idVec3& wishDirection, + const float wishSpeed, const float acceleration) { + const float currentSpeed = current.velocity.Dot(wishDirection); + const float addSpeed = wishSpeed - currentSpeed; + if (addSpeed <= 0.0f) return; + const float accelerationSpeed = (std::min)(addSpeed, + acceleration * frametime * wishSpeed); + current.velocity = current.velocity + wishDirection * accelerationSpeed; +} + +void idPhysics_Player::UpdateExternalVelocity() { + current.velocity = current.velocity + externalVelocity; + externalVelocity.Zero(); +} + +void idPhysics_Player::Friction() { + idVec3 velocity = current.velocity; + if (walking) ProjectOntoPlane(velocity, gravityNormal); + const float speed = velocity.Length(); + if (speed < 1.0f) { + if (walking) ProjectOntoPlane(current.velocity, -gravityNormal); + return; + } + float coefficient = walking ? walkFriction : airFriction; + if (current.movementType == PM_SPECTATOR + || current.movementType == PM_NOCLIP) + coefficient = flyFriction; + if (waterLevel > 0.0f) coefficient = waterFriction * waterLevel; + if (current.movementType == PM_SLIDE) coefficient = slideFriction; + const float newSpeed = (std::max)(0.0f, + speed - speed * coefficient * frametime); + if (newSpeed != speed) current.velocity = current.velocity + * (newSpeed / speed); +} + +bool idPhysics_Player::CheckJump() { + if (inhibitJump || !walking || command.upmove < 10) return false; + if (prevcmd.upmove >= 10 && current.movementTime <= 0) return false; + const float gravity = gravityVector.Length(); + const float jumpSpeed = std::sqrt((std::max)(0.0f, + 2.0f * gravity * maxJumpHeight)); + ProjectOntoPlane(current.velocity, gravityNormal); + current.velocity = current.velocity - gravityNormal * jumpSpeed; + current.movementFlags |= PMF_JUMPED; + walking = false; + groundPlane = false; + if (ownerJumpHandler != nullptr) + ownerJumpHandler(GetEntityNumber(), ¤t.worldOrigin, + ¤t.velocity, &viewForward, &viewRight, &command); + return true; +} + +void idPhysics_Player::CorrectAllSolid(trace_t& trace, int) { + trace.fraction = 0.0f; + trace.endpos = current.worldOrigin; + current.velocity.Zero(); +} + +void idPhysics_Player::SlideMoveDeferred(const bool gravity, + const bool stepUp, const bool stepDown, const bool push) { + SlideMoveNonDeferred(gravity, stepUp, stepDown, push); +} + +void idPhysics_Player::SlideMoveNonDeferred(const bool applyGravity, + const bool allowStepUp, const bool allowStepDown, bool) { + idClipModel* const model = clipModels[0]; + if (clip == nullptr || model == nullptr) { + if (applyGravity) + current.velocity = current.velocity + gravityVector * frametime; + current.worldOrigin = current.worldOrigin + + current.velocity * frametime; + current.localOrigin = current.worldOrigin; + return; + } + if (applyGravity) + current.velocity = current.velocity + gravityVector * frametime; + const idVec3 start = current.worldOrigin; + const idVec3 end = start + current.velocity * frametime; + trace_t collision{}; + collision.fraction = 1.0f; + collision.endpos = end; + collision.endAxis = clipModelAxis; + contactsResult_t result{}; + const float up = allowStepUp ? maxStepHeight : 0.0f; + const float down = allowStepDown ? maxStepHeight * 2.0f : 0.0f; + slideMoveQuery = allowStepUp + ? clip->StepMoveContacts(&collision, &result, start, end, + gravityNormal, up, down, model, clipModelAxis, clipMasks[0], + GetEntityNumber(), false, + "idPhysics_Player::SlideMoveNonDeferred") + : clip->SlideMoveContacts(&collision, &result, start, end, + gravityNormal, up, model, clipModelAxis, clipMasks[0], + GetEntityNumber(), false, + "idPhysics_Player::SlideMoveNonDeferred"); + current.worldOrigin = collision.endpos; + current.localOrigin = current.worldOrigin; + ClearContacts(); + const int count = (std::min)(12, result.numContacts); + for (int i = 0; i < count; ++i) { + contacts.Append(result.contacts[i]); + UpdateCollisionResidency(result.contacts[i]); + } + AddContactPhysicsForContacts(); + if (collision.fraction < 1.0f) { + ProjectOntoPlane(current.velocity, collision.c.normal, 1.001f); + if (callbacks != nullptr) + GameLib_NotifyPhysicsCollision(callbacks, GetPhysicsId(), + collision, current.velocity); + } + const float stepped = (collision.endpos - start).Dot(-gravityNormal); + current.stepUp = stepped > 0.0f ? stepped : 0.0f; + if (current.stepUp > 0.0f) current.movementFlags |= PMF_STEPPED_UP; + LinkClip(current.worldOrigin, clipModelAxis); +} + +void idPhysics_Player::SlideMove(const bool gravity, const bool stepUp, + const bool stepDown, const bool push) { + SlideMoveDeferred(gravity, stepUp, stepDown, push); +} + +void idPhysics_Player::CheckGround() { + walking = false; + groundPlane = false; + groundTrace = {}; + groundTrace.fraction = 1.0f; + groundTrace.endpos = current.worldOrigin; + groundTrace.endAxis = clipModelAxis; + idClipModel* const model = clipModels[0]; + if (clip == nullptr || model == nullptr) return; + const idVec3 end = current.worldOrigin + gravityNormal + * (maxStepHeight > 0.0f ? maxStepHeight : 2.0f); + clip->Translation(&groundTrace, current.worldOrigin, end, model, + clipModelAxis, clipMasks[0], GetEntityNumber(), false, + "idPhysics_Player::CheckGround"); + if (groundTrace.fraction < 1.0f) { + groundPlane = true; + walking = groundTrace.c.normal.Dot(-gravityNormal) + >= MIN_FLOOR_COSINE; + groundSurfaceFlags = groundTrace.c.surfaceFlags; + UpdateCollisionResidency(groundTrace.c); + if (walking) { + bool present = false; + for (int i = 0; i < contacts.Num(); ++i) + present |= contacts[i].entityNum == groundTrace.c.entityNum; + if (!present) contacts.Append(groundTrace.c); + } + } +} + +bool idPhysics_Player::CanStand() { + return CanStand(current.worldOrigin); +} + +bool idPhysics_Player::CanStand(const idVec3& position) { + if (clip == nullptr || clipModel_standing == nullptr) return true; + trace_t result{}; + contentsQuery = clip->Contents(&result, position, clipModel_standing, + clipModelAxis, clipMasks[0], GetEntityNumber(), + "idPhysics_Player::CanStand"); + return result.c.contentFlags == 0 && result.fraction != 0.0f; +} + +void idPhysics_Player::CheckLadder(const bool oldLadder) { + if (!canLadder || clip == nullptr || clipModels[0] == nullptr) { + ladder = false; + return; + } + trace_t result{}; + const idVec3 end = current.worldOrigin + viewForward * 2.0f; + ladderQuery1 = clip->Translation(&result, current.worldOrigin, end, + clipModels[0], clipModelAxis, clipMasks[0], GetEntityNumber(), + false, "idPhysics_Player::CheckLadder"); + ladder = result.fraction < 1.0f + && std::fabs(result.c.normal.Dot(gravityNormal)) < 0.5f; + if (ladder) ladderNormal = result.c.normal; + else if (oldLadder) ladderNormal.Zero(); +} + +void idPhysics_Player::NoclipMove() { + Friction(); + const float scale = CmdScale(command); + idVec3 up = -gravityNormal; + idVec3 wish = viewForward * static_cast(command.forwardmove) + + viewRight * static_cast(command.rightmove) + + up * static_cast(command.upmove); + const float length = wish.NormalizeFast(); + if (length > 0.0f) Accelerate(wish, scale * length, flyAccelerate); + current.worldOrigin = current.worldOrigin + current.velocity * frametime; + current.localOrigin = current.worldOrigin; + LinkClip(current.worldOrigin, clipModelAxis); +} + +void idPhysics_Player::WaterJumpMove() { + SlideMove(true, true, true, false); +} + +void idPhysics_Player::SwimMove() { + Friction(); + idVec3 up = -gravityNormal; + idVec3 wish = viewForward * static_cast(command.forwardmove) + + viewRight * static_cast(command.rightmove) + + up * static_cast(command.upmove); + float wishSpeed = CmdScale(command) * wish.NormalizeFast(); + if (!swimStrokeIgnoreInput) Accelerate(wish, wishSpeed, waterAccelerate); + if (swimStrokeTimeLeft > 0) { + current.velocity = current.velocity + swimStrokeVelocity + * frametime; + swimStrokeTimeLeft -= framemsec; + } + current.velocity = current.velocity + + gravityVector * (frametime * (1.0f - waterLevel)); + SlideMove(false, false, false, false); +} + +void idPhysics_Player::AirMove() { + Friction(); + idVec3 forward = viewForward; + idVec3 right = viewRight; + ProjectOntoPlane(forward, gravityNormal); + ProjectOntoPlane(right, gravityNormal); + forward.NormalizeFast(); + right.NormalizeFast(); + idVec3 wish = forward * static_cast(command.forwardmove) + + right * static_cast(command.rightmove); + const float length = wish.NormalizeFast(); + Accelerate(wish, CmdScale(command) * length, airAccelerate); + SlideMove(true, false, false, false); +} + +void idPhysics_Player::WalkMove() { + if (CheckJump()) { + AirMove(); + return; + } + Friction(); + idVec3 forward = viewForward; + idVec3 right = viewRight; + ProjectOntoPlane(forward, groundTrace.c.normal); + ProjectOntoPlane(right, groundTrace.c.normal); + forward.NormalizeFast(); + right.NormalizeFast(); + idVec3 wish = forward * static_cast(command.forwardmove) + + right * static_cast(command.rightmove); + const float length = wish.NormalizeFast(); + playerSpeed = IsCrouching() ? crouchSpeed : walkSpeed; + Accelerate(wish, CmdScale(command) * length, walkAccelerate); + ProjectOntoPlane(current.velocity, groundTrace.c.normal); + SlideMove(false, true, true, true); +} + +void idPhysics_Player::PerfectOriginMove(const int deltaMS) { + perfectOriginTime += deltaMS; + const float fraction = perfectOriginDuration > 0 + ? (std::min)(1.0f, static_cast(perfectOriginTime) + / static_cast(perfectOriginDuration)) : 1.0f; + const idVec3 start = current.worldOrigin; + current.worldOrigin = start + + (perfectOriginPosition - start) * fraction; + current.localOrigin = current.worldOrigin; + current.velocity = deltaMS > 0 + ? (current.worldOrigin - start) * (1000.0f / deltaMS) + : kZeroVector; + LinkClip(current.worldOrigin, clipModelAxis); + if (fraction >= 1.0f) current.movementType = PM_NORMAL; +} + +void idPhysics_Player::DeadMove() { + Friction(); + SlideMove(true, false, true, false); +} + +void idPhysics_Player::SpectatorMove() { NoclipMove(); } + +void idPhysics_Player::LadderMove() { + Friction(); + idVec3 up = -gravityNormal; + idVec3 side = up.Cross(ladderNormal); + side.NormalizeFast(); + idVec3 wish = up * static_cast(command.forwardmove + + command.upmove) + side * static_cast(command.rightmove); + const float length = wish.NormalizeFast(); + Accelerate(wish, CmdScale(command) * length, flyAccelerate); + ProjectOntoPlane(current.velocity, ladderNormal, 1.001f); + SlideMove(false, false, false, false); +} + +void idPhysics_Player::ExplicitMove(playerExplicitMove_t& move) { + if (move.velocityType == playerExplicitMove_t::VEL_OVERRIDE) + current.velocity = move.overrideVelocity; + else if (move.velocityType == playerExplicitMove_t::VEL_EXPLICIT + && frametime > 0.0f) + current.velocity = move.delta * (1.0f / frametime); + if (move.clipMove) { + const idVec3 originalVelocity = current.velocity; + current.velocity = move.delta * (frametime > 0.0f + ? 1.0f / frametime : 0.0f); + SlideMove(false, false, false, false); + if (move.velocityType == playerExplicitMove_t::VEL_CURRENT) + current.velocity = originalVelocity; + } else { + current.worldOrigin = current.worldOrigin + move.delta; + current.localOrigin = current.worldOrigin; + LinkClip(current.worldOrigin, clipModelAxis); + } + move.delta.Zero(); +} + +void idPhysics_Player::CheckDuck() { + const bool sprint = (current.movementFlags & PMF_SPRINT_SLIDE) != 0; + bool wantCrouch = forcedClientCrouchState || toggleCrouch + || command.upmove < 0 || sprint; + if (!wantCrouch && !CanStand()) wantCrouch = true; + if (wantCrouch) current.movementFlags |= PMF_DUCKED; + else current.movementFlags &= ~PMF_DUCKED; + idClipModel* desired = clipModel_standing; + if (current.movementType == PM_DEAD) desired = clipModel_dead; + else if (sprint && clipModel_sprintSlide != nullptr) + desired = clipModel_sprintSlide; + else if (wantCrouch) desired = clipModel_crouched; + SelectClipModel(desired); +} + +void idPhysics_Player::ForcePhysicsQuery() { + slideMoveQuery.index = 0; + ladderQuery1.index = 0; + ladderQuery2.index = 0; + contentsQuery.index = 0; + CheckGround(); +} + +void idPhysics_Player::ClearCrouch() { + forcedClientCrouchState = false; + toggleCrouch = false; + if (CanStand()) { + current.movementFlags &= ~PMF_DUCKED; + SelectClipModel(clipModel_standing); + } +} + +void idPhysics_Player::SetCrouch(const bool crouch) { + toggleCrouch = crouch; + CheckDuck(); + if (ownerCrouchHandler != nullptr) + ownerCrouchHandler(GetEntityNumber(), ¤t.worldOrigin, + ¤t.velocity, &viewForward, &viewRight, &command); +} + +void idPhysics_Player::ResolveSlideMove() { + if (clientOriginSet) UpdateNonPredictiveClip(); +} + +void idPhysics_Player::MovePlayer(const int milliseconds) { + framemsec = (std::max)(0, milliseconds); + frametime = framemsec * 0.001f; + previous = current; + current.stepUp = 0.0f; + current.movementFlags &= ~(PMF_JUMPED | PMF_STEPPED_UP); + if (current.movementTime > 0) + current.movementTime = (std::max)(0, + current.movementTime - framemsec); + viewAngles.ToVectors(&viewForward, &viewRight, nullptr); + UpdateExternalVelocity(); + CheckDuck(); + startedOnGround = walking; + CheckGround(); + const bool oldLadder = ladder; + CheckLadder(oldLadder); + + switch (current.movementType) { + case PM_FREEZE: + current.velocity.Zero(); + break; + case PM_NOCLIP: + case PM_VEHICLE_NOCLIP: + NoclipMove(); + break; + case PM_SPECTATOR: + SpectatorMove(); + break; + case PM_DEAD: + DeadMove(); + break; + case PM_PERFECTORIGIN: + PerfectOriginMove(milliseconds); + break; + case PM_EXPLICIT: + ExplicitMove(explicitMove); + ExplicitMove(explicitMove2); + break; + case PM_SLIDE: + Friction(); + SlideMove(true, false, true, false); + break; + case PM_NORMAL: + default: + if (ladder) LadderMove(); + else if (waterLevel > 0.5f) SwimMove(); + else if (walking) WalkMove(); + else AirMove(); + break; + } + ResolveSlideMove(); + CheckGround(); +} + +bool idPhysics_Player::Evaluate(const int timeStepMSec, int) { + RememberHistorySample(); + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) { + const idVec3 oldOrigin = current.worldOrigin; + current.worldOrigin = masterOrigin + + masterAxis * current.localOrigin; + LinkClip(current.worldOrigin, clipModelAxis); + const float seconds = timeStepMSec * 0.001f; + if (seconds > 0.0f) + current.velocity = (current.worldOrigin - oldOrigin) + * (1.0f / seconds); + masterDeltaYaw = AxisYaw(masterAxis) - masterYaw; + masterYaw += masterDeltaYaw; + } + } else MovePlayer(timeStepMSec); + if (IsOutsideWorld() && callbacks != nullptr) + GameLib_NotifyPhysicsDeactivated(callbacks, GetPhysicsId()); + return true; +} + diff --git a/source/engine/gamelib/physics/physics_player.h b/source/engine/gamelib/physics/physics_player.h index a4e0859..c046970 100644 --- a/source/engine/gamelib/physics/physics_player.h +++ b/source/engine/gamelib/physics/physics_player.h @@ -1,175 +1,279 @@ #pragma once -// Reconstructed C++ declarations from IDA Local Types and PDB/DIA metadata. -// Original PDB header: w:\tech5\engine\gamelib\physics\physics_player.h -// Recovered logical types: 1 -// Signatures retain Xbox 360 ABI evidence and may still require manual review. +#include "gamelib/physics/clip.h" +#include "gamelib/physics/physics_actor.h" +#include "idlib/math/angles.h" +#include -// IDA Local Type ordinal 15364; PDB kind: class. -class __declspec(align(8)) idPhysics_Player : public idPhysics_Actor -{ -public: - // Recovered virtual interface; IDA vtable ordinal 15365. - virtual ~idPhysics_Player(); - virtual void SetClipModel(idClipModel *, float, int, bool); - virtual idClipModel *GetClipModel(int); - virtual int GetNumClipModels(); - virtual void SetMass(float, int); - virtual float GetMass(int); - virtual void SetContents(int, int); - virtual int GetContents(int); - virtual void SetClipMask(int, int); - virtual int GetClipMask(int); - virtual const idBounds *GetBounds(int); - virtual const idBounds *GetAbsBounds(int); - virtual void SetOrigin(const idVec3 *, int); - virtual void SetAxis(const idMat3 *, int); - virtual void Translate(const idVec3 *, int); - virtual void Rotate(const idRotation *, int); - virtual const idVec3 *GetOrigin(int); - virtual const idMat3 *GetAxis(int); - virtual const idVec3 *GetLocalOrigin(int); - virtual const idMat3 *GetLocalAxis(int); - virtual void SetLinearVelocity(const idVec3 *, int); - virtual void SetAngularVelocity(const idVec3 *, int); - virtual idVec3 *GetLinearVelocity(idVec3 *result, int); - virtual idVec3 *GetAngularVelocity(idVec3 *result, int); - virtual void SetGravity(const idVec3 *); - virtual const idVec3 *GetGravity(); - virtual const idVec3 *GetGravityNormal(); - virtual void SetWaterLevel(float, int); - virtual float GetWaterLevel(int); - virtual void SetWaterViscosity(float, int); - virtual float GetWaterViscosity(int); - virtual void SetWaterEntNum(int); - virtual int GetWaterEntNum(); - virtual void SetWaterSurfaceWrldHeight(float); - virtual float GetWaterSurfaceWrldHeight(); - virtual void GetImpactInfo(const int, const idVec3 *, impactInfo_t *); - virtual void ApplyImpulse(const int, const idVec3 *, const idVec3 *); - virtual void ApplyForce(const int, const idVec3 *, const idVec3 *); - virtual void Activate(); - virtual void PutToRest(); - virtual bool IsAtRest(); - virtual bool IsPushable(int); - virtual void SaveState(); - virtual void RestoreState(); - virtual bool Evaluate(int, int); - virtual void UpdateTime(int); - virtual void ClipTranslation(trace_t *, const idVec3 *, const idClipModel *); - virtual void ClipRotation(trace_t *, const idRotation *, const idClipModel *); - virtual int ClipContents(const idClipModel *, int); - virtual void DisableClip(); - virtual void EnableClip(); - virtual void UnlinkClip(); - virtual void LinkClip(); - virtual bool EvaluateContacts(); - virtual int GetNumContacts(); - virtual const contactInfo_t *GetContact(int); - virtual void ClearContacts(); - virtual void AddContactPhysics(idPhysics *); - virtual void RemoveContactPhysics(idPhysics *); - virtual int GetNumContactPhysics(); - virtual idPhysics *GetContactPhysics(int); - virtual void ActivateContactPhysics(); - virtual bool HasGroundContacts(); - virtual bool IsGroundEntity(int); - virtual bool IsGroundClipModel(int, int); - virtual void SetPushed(int); - virtual idVec3 *GetPushedLinearVelocity(idVec3 *result, const int); - virtual idVec3 *GetPushedAngularVelocity(idVec3 *result, const int); - virtual void SetMaster(bool, const idVec3 *, const idMat3 *, const bindFlags_t); - virtual void SetLocalOrigin(const idVec3 *, int); - virtual void SetLocalAxis(const idMat3 *, int); - virtual int GetBlockingEntityNum(); - virtual int GetLinearEndTime(); - virtual int GetAngularEndTime(); - virtual bool IsInNonResidentCollisionArea(bool); - virtual bool IsOutsideWorld(); - virtual const idMat3 *GetGravityAxis(); - virtual void DisableClip_2(const idPhysics_Actor::actorClipModel_t); - virtual void EnableClip_2(const idPhysics_Actor::actorClipModel_t); - virtual void LinkClip_2(const idVec3 *, const idMat3 *); - virtual void RestoreStateFromState(playerPState_t *); - virtual void ClearPushedVelocity(); - virtual void Serialize(idSerializer *, playerPState_t *); - virtual void SerializeNonPredictiveState(idSerializer *); +class idMaterial; +class idPush; +class idSerializer; - playerExplicitMove_t explicitMove; - playerExplicitMove_t explicitMove2; - playerPState_t current; - playerPState_t previous; - playerPState_t saved; - idClipModel *clipModel_standing; - idClipModel *clipModel_crouched; - idClipModel *clipModel_sprintSlide; - idClipModel *clipModel_dead; - const idMaterial *overrideClipMaterial; - idPush *pusher; - idClipQuery slideMoveQuery; - idClipQuery ladderQuery1; - idClipQuery ladderQuery2; - idClipQuery contentsQuery; - int lastContents; - float walkSpeed; - float crouchSpeed; - float maxStepHeight; - float maxJumpHeight; - int debugLevel; - float walkAccelerate; - float airAccelerate; - float flyAccelerate; - float waterAccelerate; - float walkFriction; - float airFriction; - float flyFriction; - float waterFriction; - float slideFriction; - usercmd_t command; - usercmd_t prevcmd; - idAngles viewAngles; - int framemsec; - float frametime; - float playerSpeed; - idVec3 viewForward; - idVec3 viewRight; - bool walking; - bool startedOnGround; - bool groundPlane; - trace_t groundTrace; - int groundSurfaceFlags; - bool inhibitSprint; - bool inhibitJump; - bool toggleCrouch; - bool ladder; - idVec3 ladderNormal; - bool canLadder; - swimdepthstate_t currentSwimDepthState; - idVec3 swimStrokeVelocity; - int swimStrokeTimeLeft; - int swimStrokeTime; - bool swimStrokeAltCurve; - bool swimStrokeIgnoreInput; - int currentWaterEnt; - float currentWaterSurfaceHeight; - float blockedAccelerationPercent; - idVec3 blockedAccelerationVec; - float modifiedMovementMaxPercent; - float modifiedMovementAngle; - idVec3 modifiedAccelerationVec; - bool canCrouchSprint; - int lastJumpChance; - idVec3 perfectOriginPosition; - int perfectOriginDuration; - int perfectOriginTime; - idVec3 externalVelocity; - bool clientOriginSet; - idVec3 nextClientOrigin; - bool forcedClientCrouchState; - bool clientPusherLocked; - float pushDeltaZ; - bool (__fastcall *ownerJumpHandler)(int, const idVec3 *, const idVec3 *, const idVec3 *, const idVec3 *, const usercmd_t *); - bool (__fastcall *ownerCrouchHandler)(int, const idVec3 *, const idVec3 *, const idVec3 *, const idVec3 *, const usercmd_t *); - bool (__fastcall *ownerCrouchJumpHandler)(int, const idVec3 *, const idVec3 *, const idVec3 *, const idVec3 *, const usercmd_t *); +enum pmtype_t : int { + PM_NORMAL = 0, + PM_DEAD = 1, + PM_SPECTATOR = 2, + PM_FREEZE = 3, + PM_NOCLIP = 4, + PM_VEHICLE_NOCLIP = 5, + PM_PERFECTORIGIN = 6, + PM_EXPLICIT = 7, + PM_SLIDE = 8, + PM_MAX = 9 }; + +enum swimdepthstate_t : int { + SWIMDEPTH_STATE_NONE = 0, + SWIMDEPTH_STATE_ON_SURFACE = 1, + SWIMDEPTH_STATE_UNDER_SURFACE = 2 +}; + +struct playerExplicitMove_t { + enum velocityType_t : int { + VEL_EXPLICIT = 0, + VEL_OVERRIDE = 1, + VEL_CURRENT = 2 + }; + + idVec3 delta; + bool clipMove; + velocityType_t velocityType; + idVec3 overrideVelocity; + int ignoreEntityNum; +}; + +struct playerPState_t { + idVec3 localOrigin; + idVec3 worldOrigin; + idVec3 velocity; + idVec3 pushVelocity; + idAngles deltaViewAngles; + float stepUp; + pmtype_t movementType; + int movementFlags; + int movementTime; +}; + +#ifndef ID_USERCMD_T_DEFINED +#define ID_USERCMD_T_DEFINED +struct alignas(4) usercmd_t { + int buttons; + std::int16_t buttons2; + char forwardmove; + char rightmove; + char upmove; + std::uint8_t reserved : 6; + std::uint8_t fromBot : 1; + std::uint8_t inhibited : 1; + std::int16_t angles[3]; + int clientGameFrame; + float pos[3]; + std::int16_t pitch; + std::int16_t yaw; + std::int16_t roll; + std::uint8_t predictionStateBits; + int serverGameTime; + std::uint16_t fireCount; + std::int16_t fireAngles[2]; + std::int16_t firePos[3]; + std::uint8_t vehicleThrottle; + std::uint8_t speed; +}; +#endif + +class idPhysics_Player : public idPhysics_Actor { +public: + using playerMoveHandler_t = bool (*)(int, const idVec3*, + const idVec3*, const idVec3*, const idVec3*, const usercmd_t*); + + idPhysics_Player(); + ~idPhysics_Player() override; + + void SetClipModel(idClipModel*, float, int, bool) override; + void SetOrigin(const idVec3*, int) override; + void SetAxis(const idMat3*, int) override; + void Translate(const idVec3*, int) override; + void Rotate(const idRotation*, int) override; + const idVec3* GetLocalOrigin(int) override; + const idMat3* GetLocalAxis(int) override; + void SetLinearVelocity(const idVec3*, int) override; + void SetAngularVelocity(const idVec3*, int) override; + idVec3* GetLinearVelocity(idVec3*, int) override; + idVec3* GetAngularVelocity(idVec3*, int) override; + void SetWaterLevel(float, int) override; + void SetWaterEntNum(int) override; + int GetWaterEntNum() override; + void SetWaterSurfaceWrldHeight(float) override; + float GetWaterSurfaceWrldHeight() override; + void GetImpactInfo(int, const idVec3*, impactInfo_t*) override; + void ApplyImpulse(int, const idVec3*, const idVec3*) override; + void ApplyForce(int, const idVec3*, const idVec3*) override; + void Activate() override; + void PutToRest() override; + bool IsAtRest() override; + bool IsPushable(int) override; + void SaveState() override; + void RestoreState() override; + bool Evaluate(int, int) override; + void UpdateTime(int) override; + void SetPushed(int) override; + idVec3* GetPushedLinearVelocity(idVec3*, int) override; + idVec3* GetPushedAngularVelocity(idVec3*, int) override; + void SetMaster(bool, const idVec3*, const idMat3*, bindFlags_t) override; + void SetLocalOrigin(const idVec3*, int) override; + void SetLocalAxis(const idMat3*, int) override; + int GetBlockingEntityNum() override; + int GetLinearEndTime() override; + int GetAngularEndTime() override; + + void RestoreStateFromState(playerPState_t*); + void ClearPushedVelocity(); + void Serialize(idSerializer*, playerPState_t*); + void SerializeNonPredictiveState(idSerializer*); + + void SetSlideFriction(float); + bool HasJumped() const; + bool HasCrouched() const; + bool HasStoodUp() const; + bool HasSteppedUp() const; + float GetStepUp() const; + bool IsCrouching() const; + void SetClientCrouch(bool); + void SetSprintSliding(bool); + void SetClipModelStanding(idClipModel*); + void SetWalkFriction(float); + void SetSpeed(float, float); + void SetMaxStepHeight(float); + void SetMaxJumpHeight(float); + void SetMovementType(pmtype_t); + void SetKnockBack(int, bool); + void SetDebugLevel(bool); + void SetSwimStroke(const idVec3&, int, bool, bool); + bool GetIsUnderWater() const; + void StartPerfectOrigin(const idVec3&, int); + void SetClientDeferredOrigin(const idVec3&); + void SetPlayerInput(const usercmd_t&, const idAngles&); + bool ClientPusherLocked(bool*); + void UpdateNonPredictiveClip(); + bool CanStand(); + bool CanStand(const idVec3&); + void CheckGround(); + void CheckLadder(bool); + void ForcePhysicsQuery(); + void ClearCrouch(); + void SetCrouch(bool); + + playerExplicitMove_t explicitMove; + playerExplicitMove_t explicitMove2; + playerPState_t current; + playerPState_t previous; + playerPState_t saved; + idClipModel* clipModel_standing; + idClipModel* clipModel_crouched; + idClipModel* clipModel_sprintSlide; + idClipModel* clipModel_dead; + const idMaterial* overrideClipMaterial; + idPush* pusher; + idClipQuery slideMoveQuery; + idClipQuery ladderQuery1; + idClipQuery ladderQuery2; + idClipQuery contentsQuery; + int lastContents; + float walkSpeed; + float crouchSpeed; + float maxStepHeight; + float maxJumpHeight; + int debugLevel; + float walkAccelerate; + float airAccelerate; + float flyAccelerate; + float waterAccelerate; + float walkFriction; + float airFriction; + float flyFriction; + float waterFriction; + float slideFriction; + usercmd_t command; + usercmd_t prevcmd; + idAngles viewAngles; + int framemsec; + float frametime; + float playerSpeed; + idVec3 viewForward; + idVec3 viewRight; + bool walking; + bool startedOnGround; + bool groundPlane; + trace_t groundTrace; + int groundSurfaceFlags; + bool inhibitSprint; + bool inhibitJump; + bool toggleCrouch; + bool ladder; + idVec3 ladderNormal; + bool canLadder; + swimdepthstate_t currentSwimDepthState; + idVec3 swimStrokeVelocity; + int swimStrokeTimeLeft; + int swimStrokeTime; + bool swimStrokeAltCurve; + bool swimStrokeIgnoreInput; + int currentWaterEnt; + float currentWaterSurfaceHeight; + float blockedAccelerationPercent; + idVec3 blockedAccelerationVec; + float modifiedMovementMaxPercent; + float modifiedMovementAngle; + idVec3 modifiedAccelerationVec; + bool canCrouchSprint; + int lastJumpChance; + idVec3 perfectOriginPosition; + int perfectOriginDuration; + int perfectOriginTime; + idVec3 externalVelocity; + bool clientOriginSet; + idVec3 nextClientOrigin; + bool forcedClientCrouchState; + bool clientPusherLocked; + float pushDeltaZ; + playerMoveHandler_t ownerJumpHandler; + playerMoveHandler_t ownerCrouchHandler; + playerMoveHandler_t ownerCrouchJumpHandler; + +private: + float CmdScale(const usercmd_t&) const; + void Accelerate(const idVec3&, float, float); + void UpdateExternalVelocity(); + void SlideMoveDeferred(bool, bool, bool, bool); + void SlideMoveNonDeferred(bool, bool, bool, bool); + void SlideMove(bool, bool, bool, bool); + void Friction(); + void NoclipMove(); + void CorrectAllSolid(trace_t&, int); + bool CheckJump(); + void WaterJumpMove(); + void SwimMove(); + void AirMove(); + void WalkMove(); + void PerfectOriginMove(int); + void DeadMove(); + void SpectatorMove(); + void LadderMove(); + void ExplicitMove(playerExplicitMove_t&); + void CheckDuck(); + void ResolveSlideMove(); + void MovePlayer(int); + void SelectClipModel(idClipModel*); +}; + +static_assert(sizeof(playerExplicitMove_t) == 36, + "Recovered explicit player movement ABI changed"); +static_assert(sizeof(playerPState_t) == 76, + "Recovered player physics state ABI changed"); +static_assert(sizeof(usercmd_t) == 60, + "Recovered user command ABI changed"); +#if defined(_WIN32) && !defined(_WIN64) +static_assert(sizeof(idPhysics_Player) == 1352, + "Recovered idPhysics_Player ABI changed"); +#endif diff --git a/source/engine/gamelib/physics/physics_rigidbody.cpp b/source/engine/gamelib/physics/physics_rigidbody.cpp new file mode 100644 index 0000000..6e7a8c8 --- /dev/null +++ b/source/engine/gamelib/physics/physics_rigidbody.cpp @@ -0,0 +1,606 @@ +#include "gamelib/physics/physics_rigidbody.h" + +#include "gamelib/physics/clipmodel.h" +#include "idlib/math/rotation.h" + +#include +#include +#include + +bool GameLib_GetMasterPhysicsTransform(idPhysicsCallbacks* callbacks, + idVec3& origin, idMat3& axis); +void GameLib_NotifyPhysicsActivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsDeactivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsCollision(idPhysicsCallbacks* callbacks, + int physicsId, const trace_t& collision, const idVec3& velocity); + +namespace { + +const idBounds kRigidZeroBounds{{idVec3(0.0f, 0.0f, 0.0f), + idVec3(0.0f, 0.0f, 0.0f)}}; +const idVec3 kRigidZeroVector(0.0f, 0.0f, 0.0f); +const idMat3 kRigidIdentityAxis(1.0f); + +idVec3 SpatialLinear(const idVec6& value) { + return idVec3(value[0], value[1], value[2]); +} +idVec3 SpatialAngular(const idVec6& value) { + return idVec3(value[3], value[4], value[5]); +} +void SetSpatialLinear(idVec6& value, const idVec3& vector) { + value[0] = vector.x; value[1] = vector.y; value[2] = vector.z; +} +void SetSpatialAngular(idVec6& value, const idVec3& vector) { + value[3] = vector.x; value[4] = vector.y; value[5] = vector.z; +} +void ZeroSpatial(idVec6& value) { + for (int index = 0; index < 6; ++index) value[index] = 0.0f; +} + +idMat3 Inverse3x3(const idMat3& matrix) { + const float determinant = + matrix[0].x * (matrix[1].y * matrix[2].z + - matrix[1].z * matrix[2].y) + - matrix[0].y * (matrix[1].x * matrix[2].z + - matrix[1].z * matrix[2].x) + + matrix[0].z * (matrix[1].x * matrix[2].y + - matrix[1].y * matrix[2].x); + if (std::fabs(determinant) < 1.0e-12f) return idMat3(1.0f); + const float inverse = 1.0f / determinant; + return idMat3( + (matrix[1].y * matrix[2].z - matrix[1].z * matrix[2].y) * inverse, + (matrix[0].z * matrix[2].y - matrix[0].y * matrix[2].z) * inverse, + (matrix[0].y * matrix[1].z - matrix[0].z * matrix[1].y) * inverse, + (matrix[1].z * matrix[2].x - matrix[1].x * matrix[2].z) * inverse, + (matrix[0].x * matrix[2].z - matrix[0].z * matrix[2].x) * inverse, + (matrix[0].z * matrix[1].x - matrix[0].x * matrix[1].z) * inverse, + (matrix[1].x * matrix[2].y - matrix[1].y * matrix[2].x) * inverse, + (matrix[0].y * matrix[2].x - matrix[0].x * matrix[2].y) * inverse, + (matrix[0].x * matrix[1].y - matrix[0].y * matrix[1].x) * inverse); +} + +idMat3 WorldInverseInertia(const idPhysics_RigidBody& body) { + return body.current.worldAxis * body.inverseInertiaTensor + * body.current.worldAxis.Transpose(); +} + +} // namespace + +idPhysics_RigidBody::idPhysics_RigidBody() + : idPhysics_DynamicBase() + , clipModel(nullptr) + , linearFriction(0.6f) + , angularFriction(0.6f) + , contactFriction(0.0f) + , linearFrictionWater(1.0f) + , angularFrictionWater(1.0f) + , bouncyness(0.6f) + , inertiaScale(1.0f, 1.0f, 1.0f) + , mass(1.0f) + , inverseMass(1.0f) + , centerOfMass(0.0f, 0.0f, 0.0f) + , inertiaTensor(1.0f) + , inverseInertiaTensor(1.0f) + , fl{} + , current{} + , saved{} + , motionQuery{0} + , lastTimeStep(0.0f) { + type = PHYSICS_RIGIDBODY; + idPhysics_DynamicBase::SetClipMask(1, -1); + current.atRest = -1; + current.localOrigin.Zero(); + current.localAxis = idMat3(1.0f); + current.worldOrigin.Zero(); + current.worldAxis = idMat3(1.0f); + ZeroSpatial(current.spatialVelocity); + ZeroSpatial(current.externalForce); + ZeroSpatial(current.pushVelocity); + saved = current; +} + +idPhysics_RigidBody::~idPhysics_RigidBody() { + if (clipModel != nullptr) clipModel->Unlink(); + clipModel = nullptr; +} + +void idPhysics_RigidBody::SetClipModel(idClipModel* const model, + const float density, int, const bool freeOld) { + if (clipModel != nullptr && clipModel != model && freeOld) + clipModel->Delete(); + clipModel = model; + if (clipModel == nullptr) return; + current.worldOrigin = clipModel->GetOrigin(); + current.worldAxis = clipModel->GetAxis(); + current.localOrigin = current.worldOrigin; + current.localAxis = current.worldAxis; + clipModel->Link(GetEntityNumber(), GetEntityNumber(), 0, + current.worldOrigin, current.worldAxis); + idVec3 modelCenter; + idMat3 modelInertia; + float modelMass = 0.0f; + clipModel->GetMassProperties(density, modelMass, modelCenter, + modelInertia); + if (modelMass > 0.0f && std::isfinite(modelMass)) { + mass = modelMass; + centerOfMass = modelCenter; + inertiaTensor = modelInertia; + inertiaTensor[0].x *= inertiaScale.x; + inertiaTensor[1].y *= inertiaScale.y; + inertiaTensor[2].z *= inertiaScale.z; + } else { + mass = 1.0f; + centerOfMass.Zero(); + inertiaTensor = idMat3(1.0f); + } + inverseMass = 1.0f / mass; + inverseInertiaTensor = Inverse3x3(inertiaTensor); + ZeroSpatial(current.spatialVelocity); +} + +idClipModel* idPhysics_RigidBody::GetClipModel(int) { return clipModel; } +int idPhysics_RigidBody::GetNumClipModels() { return clipModel != nullptr; } + +void idPhysics_RigidBody::SetMass(const float newMass, int) { + if (newMass <= 0.0f || !std::isfinite(newMass)) return; + const float scale = newMass / mass; + for (int row = 0; row < 3; ++row) + for (int column = 0; column < 3; ++column) + inertiaTensor[row][column] *= scale; + mass = newMass; + inverseMass = 1.0f / mass; + inverseInertiaTensor = Inverse3x3(inertiaTensor); +} +float idPhysics_RigidBody::GetMass(int) { return mass; } + +void idPhysics_RigidBody::SetFriction(const float linear, + const float angular, const float contact) { + linearFriction = (std::max)(0.0f, (std::min)(1.0f, linear)); + angularFriction = (std::max)(0.0f, (std::min)(1.0f, angular)); + contactFriction = (std::max)(0.0f, (std::min)(1.0f, contact)); +} +void idPhysics_RigidBody::SetWaterFriction(const float linear, + const float angular) { + linearFrictionWater = (std::max)(0.0f, (std::min)(1.0f, linear)); + angularFrictionWater = (std::max)(0.0f, (std::min)(1.0f, angular)); +} +void idPhysics_RigidBody::SetBouncyness(const float value) { + bouncyness = (std::max)(0.0f, (std::min)(1.0f, value)); +} +void idPhysics_RigidBody::SetInertiaScale(const idVec3& scale) { + inertiaScale = scale; +} + +void idPhysics_RigidBody::SetContents(const int contents, int) { + if (clipModel != nullptr) clipModel->SetContents(contents); +} +int idPhysics_RigidBody::GetContents(int) { + return clipModel != nullptr ? clipModel->GetContents() : 0; +} +const idBounds* idPhysics_RigidBody::GetBounds(int) { + return clipModel != nullptr ? &clipModel->GetBounds() : &kRigidZeroBounds; +} +const idBounds* idPhysics_RigidBody::GetAbsBounds(int) { + return clipModel != nullptr ? &clipModel->GetAbsBounds() : &kRigidZeroBounds; +} + +void idPhysics_RigidBody::SetOrigin(const idVec3* const origin, int) { + if (origin == nullptr) return; + current.worldOrigin = *origin; + if (fl.hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current.localOrigin = masterAxis.Transpose() + * (*origin - masterOrigin); + } else { + current.localOrigin = *origin; + } + LinkClip(); + Activate(); +} +void idPhysics_RigidBody::SetAxis(const idMat3* const axis, int) { + if (axis == nullptr) return; + current.worldAxis = *axis; + if (fl.hasMaster && fl.isOrientated && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current.localAxis = *axis * masterAxis.Transpose(); + } else { + current.localAxis = *axis; + } + LinkClip(); + Activate(); +} +void idPhysics_RigidBody::Translate(const idVec3* const translation, int) { + if (translation == nullptr) return; + ResolveCollisions(); + current.worldOrigin = current.worldOrigin + *translation; + current.localOrigin = current.localOrigin + *translation; + LinkClip(); + Activate(); +} +void idPhysics_RigidBody::Rotate(const idRotation* const rotation, int) { + if (rotation == nullptr) return; + ResolveCollisions(); + current.worldOrigin = *rotation * current.worldOrigin; + current.worldAxis *= rotation->ToMat3(); + current.localOrigin = *rotation * current.localOrigin; + current.localAxis *= rotation->ToMat3(); + LinkClip(); + Activate(); +} +const idVec3* idPhysics_RigidBody::GetOrigin(int) { + return ¤t.worldOrigin; +} +const idMat3* idPhysics_RigidBody::GetAxis(int) { return ¤t.worldAxis; } +const idVec3* idPhysics_RigidBody::GetLocalOrigin(int) { + return ¤t.localOrigin; +} +const idMat3* idPhysics_RigidBody::GetLocalAxis(int) { + return ¤t.localAxis; +} + +void idPhysics_RigidBody::SetLinearVelocity(const idVec3* const value, int) { + if (value == nullptr) return; + SetSpatialLinear(current.spatialVelocity, *value); + Activate(); +} +void idPhysics_RigidBody::SetAngularVelocity(const idVec3* const value, int) { + if (value == nullptr) return; + SetSpatialAngular(current.spatialVelocity, *value); + Activate(); +} +idVec3* idPhysics_RigidBody::GetLinearVelocity(idVec3* const result, int) { + if (result != nullptr) *result = SpatialLinear(current.spatialVelocity); + return result; +} +idVec3* idPhysics_RigidBody::GetAngularVelocity(idVec3* const result, int) { + if (result != nullptr) *result = SpatialAngular(current.spatialVelocity); + return result; +} +void idPhysics_RigidBody::SetWaterEntNum(int) {} +int idPhysics_RigidBody::GetWaterEntNum() { return -1; } +void idPhysics_RigidBody::SetWaterSurfaceWrldHeight(float) {} +float idPhysics_RigidBody::GetWaterSurfaceWrldHeight() { return 0.0f; } + +void idPhysics_RigidBody::GetImpactInfo(int, const idVec3* const point, + impactInfo_t* const info) { + if (info == nullptr) return; + info->Zero(); + info->invMass = inverseMass; + info->invInertiaTensor = WorldInverseInertia(*this); + const idVec3 worldCenter = current.worldOrigin + + current.worldAxis * centerOfMass; + info->position = point != nullptr ? *point - worldCenter : kRigidZeroVector; + info->velocity = SpatialLinear(current.spatialVelocity) + + SpatialAngular(current.spatialVelocity).Cross(info->position); +} + +void idPhysics_RigidBody::ApplyImpulse(int, const idVec3* const point, + const idVec3* const impulse) { + if (fl.noImpact || impulse == nullptr) return; + idVec3 linear = SpatialLinear(current.spatialVelocity) + + *impulse * inverseMass; + idVec3 angular = SpatialAngular(current.spatialVelocity); + if (point != nullptr) { + const idVec3 center = current.worldOrigin + + current.worldAxis * centerOfMass; + angular = angular + WorldInverseInertia(*this) + * ((*point - center).Cross(*impulse)); + } + SetSpatialLinear(current.spatialVelocity, linear); + SetSpatialAngular(current.spatialVelocity, angular); + CapVelocity(); + Activate(); +} +void idPhysics_RigidBody::ApplyForce(int, const idVec3* const point, + const idVec3* const force) { + if (force == nullptr) return; + SetSpatialLinear(current.externalForce, + SpatialLinear(current.externalForce) + *force); + if (point != nullptr) { + const idVec3 center = current.worldOrigin + + current.worldAxis * centerOfMass; + SetSpatialAngular(current.externalForce, + SpatialAngular(current.externalForce) + + (*point - center).Cross(*force)); + } + Activate(); +} + +void idPhysics_RigidBody::Activate() { + current.atRest = -1; + if (callbacks != nullptr) + GameLib_NotifyPhysicsActivated(callbacks, GetPhysicsId()); +} +void idPhysics_RigidBody::PutToRest() { + current.atRest = 1; + ZeroSpatial(current.spatialVelocity); + ZeroSpatial(current.externalForce); +} +bool idPhysics_RigidBody::IsAtRest() { return current.atRest >= 0; } +bool idPhysics_RigidBody::IsPushable(const int sourceContentType) { + return (sourceContentType & fl.noPushMask) == 0; +} +void idPhysics_RigidBody::SaveState() { saved = current; } +void idPhysics_RigidBody::RestoreState() { + current = saved; + LinkClip(); +} +void idPhysics_RigidBody::UpdateTime(int) {} + +void idPhysics_RigidBody::ClipRotation(trace_t* const results, + const idRotation* const rotation, const idClipModel* const model) { + if (results == nullptr) return; + const idClipModel* moving = model != nullptr ? model : clipModel; + if (clip == nullptr || moving == nullptr || rotation == nullptr) { + std::memset(results, 0, sizeof(*results)); + results->fraction = 1.0f; + return; + } + clip->Rotation(results, current.worldOrigin, *rotation, moving, + current.worldAxis, clipMask, GetEntityNumber(), false, + "idPhysics_RigidBody::ClipRotation"); +} +int idPhysics_RigidBody::ClipContents(const idClipModel* const model, + int queryMask) { + if (clip == nullptr || clipModel == nullptr) return 0; + if (queryMask == 0) queryMask = clipMask; + trace_t result{}; + if (model != nullptr) + clip->ContentsModel(result, current.worldOrigin, clipModel, + current.worldAxis, queryMask, model->GetOrigin(), model, + model->GetAxis()); + else + clip->Contents(&result, current.worldOrigin, clipModel, + current.worldAxis, queryMask, GetEntityNumber(), + "idPhysics_RigidBody::ClipContents"); + return result.c.contentFlags; +} +void idPhysics_RigidBody::DisableClip() { + if (clipModel != nullptr) clipModel->Disable(); +} +void idPhysics_RigidBody::EnableClip() { + if (clipModel != nullptr) clipModel->Enable(); +} +void idPhysics_RigidBody::UnlinkClip() { + if (clipModel != nullptr) clipModel->Unlink(); +} +void idPhysics_RigidBody::LinkClip() { + if (clipModel != nullptr) + clipModel->Link(GetEntityNumber(), GetEntityNumber(), + clipModel->GetBodyId(), current.worldOrigin, current.worldAxis); +} +bool idPhysics_RigidBody::EvaluateContacts() { + ClearContacts(); + AddGroundContacts(clipModel, 12); + return contacts.Num() != 0; +} +bool idPhysics_RigidBody::IsGroundClipModel(const int entityNum, + const int id) { + return idPhysics_DynamicBase::IsGroundClipModel(entityNum, id); +} + +void idPhysics_RigidBody::SetPushed(const int deltaTime) { + if (deltaTime <= 0) { + ZeroSpatial(current.pushVelocity); + return; + } + const float scale = 1000.0f / static_cast(deltaTime); + SetSpatialLinear(current.pushVelocity, + (current.worldOrigin - saved.worldOrigin) * scale); + SetSpatialAngular(current.pushVelocity, + SpatialAngular(current.spatialVelocity)); +} +idVec3* idPhysics_RigidBody::GetPushedLinearVelocity(idVec3* const result, + int) { + if (result != nullptr) *result = SpatialLinear(current.pushVelocity); + return result; +} +idVec3* idPhysics_RigidBody::GetPushedAngularVelocity(idVec3* const result, + int) { + if (result != nullptr) *result = SpatialAngular(current.pushVelocity); + return result; +} + +void idPhysics_RigidBody::SetMaster(const bool enable, + const idVec3* const masterOrigin, const idMat3* const masterAxis, + const bindFlags_t flags) { + motionQuery.index = 0; + if (enable && masterOrigin != nullptr && masterAxis != nullptr) { + if (!fl.hasMaster) { + current.localOrigin = masterAxis->Transpose() + * (current.worldOrigin - *masterOrigin); + current.localAxis = (static_cast(flags) & 1) != 0 + ? current.worldAxis * masterAxis->Transpose() + : current.worldAxis; + } + fl.hasMaster = 1; + fl.isOrientated = (static_cast(flags) & 1) != 0; + ClearContacts(); + } else if (fl.hasMaster) { + fl.hasMaster = 0; + Activate(); + } +} +void idPhysics_RigidBody::SetLocalOrigin(const idVec3* const origin, int) { + if (origin == nullptr) return; + current.localOrigin = *origin; + if (fl.hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current.worldOrigin = masterOrigin + masterAxis * *origin; + } else { + current.worldOrigin = *origin; + } + LinkClip(); + Activate(); +} +void idPhysics_RigidBody::SetLocalAxis(const idMat3* const axis, int) { + if (axis == nullptr) return; + current.localAxis = *axis; + if (fl.hasMaster && fl.isOrientated && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) current.worldAxis = *axis * masterAxis; + } else { + current.worldAxis = *axis; + } + LinkClip(); + Activate(); +} + +void idPhysics_RigidBody::CapVelocity() { + idVec3 linear = SpatialLinear(current.spatialVelocity); + idVec3 angular = SpatialAngular(current.spatialVelocity); + const float linearSpeed = linear.Length(); + const float angularSpeed = angular.Length(); + if (linearSpeed > 16000.0f) linear = linear * (16000.0f / linearSpeed); + if (angularSpeed > 62.831853f) + angular = angular * (62.831853f / angularSpeed); + SetSpatialLinear(current.spatialVelocity, linear); + SetSpatialAngular(current.spatialVelocity, angular); +} + +bool idPhysics_RigidBody::TestIfAtRest() { + const idVec3 linear = SpatialLinear(current.spatialVelocity); + const idVec3 angular = SpatialAngular(current.spatialVelocity); + if (linear.LengthSqr() > 4.0f || angular.LengthSqr() > 0.01f) + return false; + if (!EvaluateContacts()) return false; + PutToRest(); + return true; +} + +void idPhysics_RigidBody::ResolveCollisions(const trace_t* const collision, + const contactsResult_t* const contactResults) { + if (contactResults != nullptr) { + ClearContacts(); + const int count = (std::min)(contactResults->numContacts, 12); + for (int index = 0; index < count; ++index) { + contacts.Append(contactResults->contacts[index]); + UpdateCollisionResidency(contactResults->contacts[index]); + } + AddContactPhysicsForContacts(); + } + if (collision == nullptr || collision->fraction >= 1.0f) return; + idVec3 linear = SpatialLinear(current.spatialVelocity); + const float intoSurface = linear.Dot(collision->c.normal); + if (intoSurface < 0.0f) + linear = linear - collision->c.normal + * ((1.0f + bouncyness) * intoSurface); + const idVec3 tangent = linear + - collision->c.normal * linear.Dot(collision->c.normal); + linear = linear - tangent * contactFriction; + SetSpatialLinear(current.spatialVelocity, linear); + if (callbacks != nullptr) + GameLib_NotifyPhysicsCollision(callbacks, GetPhysicsId(), + *collision, linear); +} + +void idPhysics_RigidBody::Evolve(const float timeStep, + trace_t* const collisionOverride, + contactsResult_t* const contactsOverride) { + if (clipModel == nullptr || clip == nullptr || timeStep <= 0.0f) return; + idVec3 linear = SpatialLinear(current.spatialVelocity); + idVec3 angular = SpatialAngular(current.spatialVelocity); + linear = linear + (gravityVector + + SpatialLinear(current.externalForce) * inverseMass) * timeStep; + angular = angular + WorldInverseInertia(*this) + * SpatialAngular(current.externalForce) * timeStep; + float linearDrag = linearFriction; + float angularDrag = angularFriction; + if (waterLevel > 0.0f) { + linearDrag += waterLevel * linearFrictionWater; + angularDrag += waterLevel * angularFrictionWater; + } + linear = linear * (std::max)(0.0f, 1.0f - linearDrag * timeStep); + angular = angular * (std::max)(0.0f, 1.0f - angularDrag * timeStep); + SetSpatialLinear(current.spatialVelocity, linear); + SetSpatialAngular(current.spatialVelocity, angular); + CapVelocity(); + + idVec3 angularAxis = angular; + const float angularSpeed = angularAxis.NormalizeFast(); + const idRotation rotation(current.worldOrigin, + angularSpeed > 0.0f ? angularAxis : idVec3(0.0f, 0.0f, 1.0f), + angularSpeed * timeStep * 57.29577951308232f); + const idVec3 end = current.worldOrigin + linear * timeStep; + trace_t localCollision{}; + localCollision.fraction = 1.0f; + localCollision.endpos = end; + localCollision.endAxis = current.worldAxis * rotation.ToMat3(); + contactsResult_t localContacts{}; + trace_t* collision = collisionOverride != nullptr + ? collisionOverride : &localCollision; + contactsResult_t* results = contactsOverride != nullptr + ? contactsOverride : &localContacts; + motionQuery = clip->MotionContacts(collision, results, + current.worldOrigin, end, rotation, 0.25f, clipModel, + current.worldAxis, clipMask, GetEntityNumber(), false, + "idPhysics_RigidBody::Evolve"); + current.worldOrigin = collision->endpos; + current.worldAxis = collision->endAxis; + LinkClip(); + lastTimeStep = timeStep; + ResolveCollisions(collision, results); +} + +void idPhysics_RigidBody::DropToFloorAndRest() { + if (!fl.dropToFloor || clipModel == nullptr || clip == nullptr) return; + fl.dropToFloor = 0; + trace_t trace{}; + const idVec3 end = current.worldOrigin + gravityNormal * 128.0f; + clip->Translation(&trace, current.worldOrigin, end, clipModel, + current.worldAxis, clipMask, GetEntityNumber(), false, + "idPhysics_RigidBody::DropToFloorAndRest"); + current.worldOrigin = trace.endpos; + LinkClip(); + EvaluateContacts(); + if (trace.fraction < 1.0f) PutToRest(); +} + +bool idPhysics_RigidBody::Evaluate(const int timeStepMSec, int) { + if (fl.hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (!GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) return false; + const idVec3 oldOrigin = current.worldOrigin; + current.worldOrigin = masterOrigin + masterAxis * current.localOrigin; + current.worldAxis = fl.isOrientated + ? current.localAxis * masterAxis : current.localAxis; + LinkClip(); + return (current.worldOrigin - oldOrigin).LengthSqr() != 0.0f; + } + DropToFloorAndRest(); + if (IsAtRest()) return false; + Evolve(timeStepMSec * 0.001f); + ZeroSpatial(current.externalForce); + if (IsOutsideWorld()) { + PutToRest(); + if (callbacks != nullptr) + GameLib_NotifyPhysicsDeactivated(callbacks, GetPhysicsId()); + } else { + TestIfAtRest(); + } + return true; +} + +int idPhysics_RigidBody::GetBlockingEntityNum() { + return contacts.Num() != 0 ? contacts[0].entityNum : 0x1FFF; +} +int idPhysics_RigidBody::GetLinearEndTime() { return 0; } +int idPhysics_RigidBody::GetAngularEndTime() { return 0; } +bool idPhysics_RigidBody::IsOutsideWorld() { + return idPhysics_DynamicBase::IsOutsideWorld(); +} diff --git a/source/engine/gamelib/physics/physics_rigidbody.h b/source/engine/gamelib/physics/physics_rigidbody.h index d3e2c26..904564c 100644 --- a/source/engine/gamelib/physics/physics_rigidbody.h +++ b/source/engine/gamelib/physics/physics_rigidbody.h @@ -1,121 +1,130 @@ #pragma once -// Reconstructed C++ declarations from IDA Local Types and PDB/DIA metadata. -// Original PDB header: w:\tech5\engine\gamelib\physics\physics_rigidbody.h -// Recovered logical types: 2 -// Signatures retain Xbox 360 ABI evidence and may still require manual review. +#include "gamelib/physics/clip.h" +#include "gamelib/physics/physics_dynamicbase.h" +#include -// IDA Local Type ordinal 15433; PDB kind: struct. -struct __declspec(align(4)) idPhysics_RigidBody::rigidBodyFlags_t -{ - int noPushMask; - unsigned __int8 : 3; - __int8 isOrientated : 1; - __int8 hasMaster : 1; - __int8 noImpact : 1; - __int8 testSolid : 1; - __int8 dropToFloor : 1; +struct rigidBodyPState_t { + int atRest; + idVec3 localOrigin; + idMat3 localAxis; + idVec3 worldOrigin; + idMat3 worldAxis; + idVec6 spatialVelocity; + idVec6 externalForce; + idVec6 pushVelocity; }; -// IDA Local Type ordinal 15435; PDB kind: class. -class __declspec(align(8)) idPhysics_RigidBody : public idPhysics_DynamicBase -{ +class idPhysics_RigidBody : public idPhysics_DynamicBase { public: - // Recovered virtual interface; IDA vtable ordinal 15436. - virtual ~idPhysics_RigidBody(); - virtual void SetClipModel(idClipModel *, float, int, bool); - virtual idClipModel *GetClipModel(int); - virtual int GetNumClipModels(); - virtual void SetMass(float, int); - virtual float GetMass(int); - virtual void SetContents(int, int); - virtual int GetContents(int); - virtual void SetClipMask(int, int); - virtual int GetClipMask(int); - virtual const idBounds *GetBounds(int); - virtual const idBounds *GetAbsBounds(int); - virtual void SetOrigin(const idVec3 *, int); - virtual void SetAxis(const idMat3 *, int); - virtual void Translate(const idVec3 *, int); - virtual void Rotate(const idRotation *, int); - virtual const idVec3 *GetOrigin(int); - virtual const idMat3 *GetAxis(int); - virtual const idVec3 *GetLocalOrigin(int); - virtual const idMat3 *GetLocalAxis(int); - virtual void SetLinearVelocity(const idVec3 *, int); - virtual void SetAngularVelocity(const idVec3 *, int); - virtual idVec3 *GetLinearVelocity(idVec3 *result, int); - virtual idVec3 *GetAngularVelocity(idVec3 *result, int); - virtual void SetGravity(const idVec3 *); - virtual const idVec3 *GetGravity(); - virtual const idVec3 *GetGravityNormal(); - virtual void SetWaterLevel(float, int); - virtual float GetWaterLevel(int); - virtual void SetWaterViscosity(float, int); - virtual float GetWaterViscosity(int); - virtual void SetWaterEntNum(int); - virtual int GetWaterEntNum(); - virtual void SetWaterSurfaceWrldHeight(float); - virtual float GetWaterSurfaceWrldHeight(); - virtual void GetImpactInfo(const int, const idVec3 *, impactInfo_t *); - virtual void ApplyImpulse(const int, const idVec3 *, const idVec3 *); - virtual void ApplyForce(const int, const idVec3 *, const idVec3 *); - virtual void Activate(); - virtual void PutToRest(); - virtual bool IsAtRest(); - virtual bool IsPushable(int); - virtual void SaveState(); - virtual void RestoreState(); - virtual bool Evaluate(int, int); - virtual void UpdateTime(int); - virtual void ClipTranslation(trace_t *, const idVec3 *, const idClipModel *); - virtual void ClipRotation(trace_t *, const idRotation *, const idClipModel *); - virtual int ClipContents(const idClipModel *, int); - virtual void DisableClip(); - virtual void EnableClip(); - virtual void UnlinkClip(); - virtual void LinkClip(); - virtual bool EvaluateContacts(); - virtual int GetNumContacts(); - virtual const contactInfo_t *GetContact(int); - virtual void ClearContacts(); - virtual void AddContactPhysics(idPhysics *); - virtual void RemoveContactPhysics(idPhysics *); - virtual int GetNumContactPhysics(); - virtual idPhysics *GetContactPhysics(int); - virtual void ActivateContactPhysics(); - virtual bool HasGroundContacts(); - virtual bool IsGroundEntity(int); - virtual bool IsGroundClipModel(int, int); - virtual void SetPushed(int); - virtual idVec3 *GetPushedLinearVelocity(idVec3 *result, const int); - virtual idVec3 *GetPushedAngularVelocity(idVec3 *result, const int); - virtual void SetMaster(bool, const idVec3 *, const idMat3 *, const bindFlags_t); - virtual void SetLocalOrigin(const idVec3 *, int); - virtual void SetLocalAxis(const idMat3 *, int); - virtual int GetBlockingEntityNum(); - virtual int GetLinearEndTime(); - virtual int GetAngularEndTime(); - virtual bool IsInNonResidentCollisionArea(bool); - virtual bool IsOutsideWorld(); + struct rigidBodyFlags_t { + int noPushMask; + std::uint8_t reserved : 3; + std::uint8_t isOrientated : 1; + std::uint8_t hasMaster : 1; + std::uint8_t noImpact : 1; + std::uint8_t testSolid : 1; + std::uint8_t dropToFloor : 1; + std::uint8_t pad[3]; + }; - idClipModel *clipModel; - float linearFriction; - float angularFriction; - float contactFriction; - float linearFrictionWater; - float angularFrictionWater; - float bouncyness; - idVec3 inertiaScale; - float mass; - float inverseMass; - idVec3 centerOfMass; - idMat3 inertiaTensor; - idMat3 inverseInertiaTensor; - idPhysics_RigidBody::rigidBodyFlags_t fl; - rigidBodyPState_t current; - rigidBodyPState_t saved; - idClipQuery motionQuery; - float lastTimeStep; + idPhysics_RigidBody(); + ~idPhysics_RigidBody() override; + + void SetClipModel(idClipModel*, float, int, bool) override; + idClipModel* GetClipModel(int) override; + int GetNumClipModels() override; + void SetMass(float, int) override; + float GetMass(int) override; + void SetContents(int, int) override; + int GetContents(int) override; + const idBounds* GetBounds(int) override; + const idBounds* GetAbsBounds(int) override; + void SetOrigin(const idVec3*, int) override; + void SetAxis(const idMat3*, int) override; + void Translate(const idVec3*, int) override; + void Rotate(const idRotation*, int) override; + const idVec3* GetOrigin(int) override; + const idMat3* GetAxis(int) override; + const idVec3* GetLocalOrigin(int) override; + const idMat3* GetLocalAxis(int) override; + void SetLinearVelocity(const idVec3*, int) override; + void SetAngularVelocity(const idVec3*, int) override; + idVec3* GetLinearVelocity(idVec3*, int) override; + idVec3* GetAngularVelocity(idVec3*, int) override; + void SetWaterEntNum(int) override; + int GetWaterEntNum() override; + void SetWaterSurfaceWrldHeight(float) override; + float GetWaterSurfaceWrldHeight() override; + void GetImpactInfo(int, const idVec3*, impactInfo_t*) override; + void ApplyImpulse(int, const idVec3*, const idVec3*) override; + void ApplyForce(int, const idVec3*, const idVec3*) override; + void Activate() override; + void PutToRest() override; + bool IsAtRest() override; + bool IsPushable(int) override; + void SaveState() override; + void RestoreState() override; + bool Evaluate(int, int) override; + void UpdateTime(int) override; + void ClipRotation(trace_t*, const idRotation*, const idClipModel*) override; + int ClipContents(const idClipModel*, int) override; + void DisableClip() override; + void EnableClip() override; + void UnlinkClip() override; + void LinkClip() override; + bool EvaluateContacts() override; + bool IsGroundClipModel(int, int) override; + void SetPushed(int) override; + idVec3* GetPushedLinearVelocity(idVec3*, int) override; + idVec3* GetPushedAngularVelocity(idVec3*, int) override; + void SetMaster(bool, const idVec3*, const idMat3*, bindFlags_t) override; + void SetLocalOrigin(const idVec3*, int) override; + void SetLocalAxis(const idMat3*, int) override; + int GetBlockingEntityNum() override; + int GetLinearEndTime() override; + int GetAngularEndTime() override; + + bool IsOutsideWorld() override; + + void SetFriction(float linear, float angular, float contact); + void SetWaterFriction(float linear, float angular); + void SetBouncyness(float bouncyness_); + void SetInertiaScale(const idVec3& scale); + void CapVelocity(); + bool TestIfAtRest(); + void Evolve(float timeStep, trace_t* collision = nullptr, + contactsResult_t* contacts = nullptr); + void ResolveCollisions(const trace_t* collision = nullptr, + const contactsResult_t* contacts = nullptr); + void DropToFloorAndRest(); + + idClipModel* clipModel; + float linearFriction; + float angularFriction; + float contactFriction; + float linearFrictionWater; + float angularFrictionWater; + float bouncyness; + idVec3 inertiaScale; + float mass; + float inverseMass; + idVec3 centerOfMass; + idMat3 inertiaTensor; + idMat3 inverseInertiaTensor; + rigidBodyFlags_t fl; + rigidBodyPState_t current; + rigidBodyPState_t saved; + idClipQuery motionQuery; + float lastTimeStep; }; + +static_assert(sizeof(rigidBodyPState_t) == 172, + "Recovered rigid-body state ABI changed"); +static_assert(sizeof(idPhysics_RigidBody::rigidBodyFlags_t) == 8, + "Recovered rigid-body flags ABI changed"); +#if defined(_WIN32) && !defined(_WIN64) +static_assert(sizeof(idPhysics_RigidBody) == 624, + "Recovered idPhysics_RigidBody ABI changed"); +#endif diff --git a/source/engine/gamelib/physics/physics_rigidbody3dof.cpp b/source/engine/gamelib/physics/physics_rigidbody3dof.cpp new file mode 100644 index 0000000..0d725a3 --- /dev/null +++ b/source/engine/gamelib/physics/physics_rigidbody3dof.cpp @@ -0,0 +1,517 @@ +#include "gamelib/physics/physics_rigidbody3dof.h" + +#include "gamelib/physics/clipmodel.h" +#include "idlib/math/rotation.h" + +#include +#include +#include + +bool GameLib_GetMasterPhysicsTransform(idPhysicsCallbacks* callbacks, + idVec3& origin, idMat3& axis); +void GameLib_NotifyPhysicsActivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsDeactivated(idPhysicsCallbacks* callbacks, + int physicsId); +void GameLib_NotifyPhysicsCollision(idPhysicsCallbacks* callbacks, + int physicsId, const trace_t& collision, const idVec3& velocity); + +namespace { + +const idBounds kThreeDofZeroBounds{{idVec3(0.0f, 0.0f, 0.0f), + idVec3(0.0f, 0.0f, 0.0f)}}; +const idVec3 kThreeDofZeroVector(0.0f, 0.0f, 0.0f); +const idMat3 kThreeDofIdentityAxis(1.0f); + +float ClampUnit(const float value) { + return (std::max)(0.0f, (std::min)(1.0f, value)); +} + +} // namespace + +idPhysics_RigidBody3DOF::bodyState_t::bodyState_t() + : atRest(-1) + , lastTimeStep(0.0f) + , localOrigin(0.0f, 0.0f, 0.0f) + , localAxis(1.0f) + , linearPushVelocity(0.0f, 0.0f, 0.0f) + , angularPushVelocity(0.0f, 0.0f, 0.0f) + , externalForce(0.0f, 0.0f, 0.0f) + , i{} { + i.position.Zero(); + i.orientation = idMat3(1.0f); + i.linearVelocity.Zero(); + i.angularVelocity.Zero(); +} + +idPhysics_RigidBody3DOF::idPhysics_RigidBody3DOF() + : idPhysics_DynamicBase() + , clipModel(nullptr) + , mass(1.0f) + , inverseMass(1.0f) + , linearFriction(1.0f) + , angularFriction(1.0f) + , contactFriction(1.0f) + , linearFrictionWater(1.0f) + , angularFrictionWater(1.0f) + , coefficientOfRestitution(1.0f) + , current() + , saved() + , random(0) + , translationQuery{0} + , flags{} { + type = PHYSICS_RIGIDBODY3DOF; +} + +idPhysics_RigidBody3DOF::~idPhysics_RigidBody3DOF() { + if (clipModel != nullptr) clipModel->Unlink(); + clipModel = nullptr; +} + +void idPhysics_RigidBody3DOF::SetCoefficientOfRestitution( + const float coefficient) { + coefficientOfRestitution = ClampUnit(coefficient); +} +void idPhysics_RigidBody3DOF::SetFriction(const float linear, + const float angular, const float contact) { + linearFriction = ClampUnit(linear); + angularFriction = ClampUnit(angular); + contactFriction = ClampUnit(contact); +} +void idPhysics_RigidBody3DOF::SetWaterFriction(const float linear, + const float angular) { + linearFrictionWater = ClampUnit(linear); + angularFrictionWater = ClampUnit(angular); +} + +void idPhysics_RigidBody3DOF::SetClipModel(idClipModel* const model, + const float density, int, const bool freeOld) { + if (clipModel != nullptr && clipModel != model && freeOld) + clipModel->Delete(); + clipModel = model; + if (clipModel == nullptr) return; + current.i.position = clipModel->GetOrigin(); + current.i.orientation = clipModel->GetAxis(); + current.localOrigin = current.i.position; + current.localAxis = current.i.orientation; + LinkClip(); + const idBounds& bounds = clipModel->GetBounds(); + const idVec3 size = bounds[1] - bounds[0]; + SetMass(size.x > 0.0f && size.y > 0.0f && size.z > 0.0f + ? size.x * size.y * size.z * density : 1.0f, 0); + current.i.linearVelocity.Zero(); + current.i.angularVelocity.Zero(); +} +idClipModel* idPhysics_RigidBody3DOF::GetClipModel(int) { + return clipModel; +} +int idPhysics_RigidBody3DOF::GetNumClipModels() { + return clipModel != nullptr; +} +void idPhysics_RigidBody3DOF::SetMass(const float newMass, int) { + mass = newMass > 0.0f && std::isfinite(newMass) ? newMass : 1.0f; + inverseMass = 1.0f / mass; +} +float idPhysics_RigidBody3DOF::GetMass(int) { return mass; } +void idPhysics_RigidBody3DOF::SetContents(const int contents, int) { + if (clipModel != nullptr) clipModel->SetContents(contents); +} +int idPhysics_RigidBody3DOF::GetContents(int) { + return clipModel != nullptr ? clipModel->GetContents() : 0; +} +const idBounds* idPhysics_RigidBody3DOF::GetBounds(int) { + return clipModel != nullptr ? &clipModel->GetBounds() + : &kThreeDofZeroBounds; +} +const idBounds* idPhysics_RigidBody3DOF::GetAbsBounds(int) { + return clipModel != nullptr ? &clipModel->GetAbsBounds() + : &kThreeDofZeroBounds; +} + +void idPhysics_RigidBody3DOF::SetOrigin(const idVec3* const origin, int) { + if (origin == nullptr) return; + current.i.position = *origin; + if (flags.hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current.localOrigin = masterAxis.Transpose() + * (*origin - masterOrigin); + } else { + current.localOrigin = *origin; + } + LinkClip(); + Activate(); +} +void idPhysics_RigidBody3DOF::SetAxis(const idMat3* const axis, int) { + if (axis == nullptr) return; + current.i.orientation = *axis; + current.localAxis = *axis; + LinkClip(); + Activate(); +} +void idPhysics_RigidBody3DOF::Translate(const idVec3* const translation, + int) { + if (translation == nullptr) return; + current.i.position = current.i.position + *translation; + current.localOrigin = current.localOrigin + *translation; + LinkClip(); + Activate(); +} +void idPhysics_RigidBody3DOF::Rotate(const idRotation* const rotation, int) { + if (rotation == nullptr) return; + current.i.position = *rotation * current.i.position; + current.i.orientation *= rotation->ToMat3(); + current.localOrigin = *rotation * current.localOrigin; + current.localAxis *= rotation->ToMat3(); + LinkClip(); + Activate(); +} +const idVec3* idPhysics_RigidBody3DOF::GetOrigin(int) { + return ¤t.i.position; +} +const idMat3* idPhysics_RigidBody3DOF::GetAxis(int) { + return ¤t.i.orientation; +} +const idVec3* idPhysics_RigidBody3DOF::GetLocalOrigin(int) { + return ¤t.localOrigin; +} +const idMat3* idPhysics_RigidBody3DOF::GetLocalAxis(int) { + return ¤t.localAxis; +} +void idPhysics_RigidBody3DOF::SetLinearVelocity(const idVec3* const value, + int) { + if (value == nullptr) return; + current.i.linearVelocity = *value; + Activate(); +} +void idPhysics_RigidBody3DOF::SetAngularVelocity(const idVec3* const value, + int) { + if (value == nullptr) return; + current.i.angularVelocity = *value; + Activate(); +} +idVec3* idPhysics_RigidBody3DOF::GetLinearVelocity(idVec3* const result, + int) { + if (result != nullptr) *result = current.i.linearVelocity; + return result; +} +idVec3* idPhysics_RigidBody3DOF::GetAngularVelocity(idVec3* const result, + int) { + if (result != nullptr) *result = current.i.angularVelocity; + return result; +} +void idPhysics_RigidBody3DOF::SetWaterEntNum(int) {} +int idPhysics_RigidBody3DOF::GetWaterEntNum() { return -1; } +void idPhysics_RigidBody3DOF::SetWaterSurfaceWrldHeight(float) {} +float idPhysics_RigidBody3DOF::GetWaterSurfaceWrldHeight() { return 0.0f; } + +void idPhysics_RigidBody3DOF::GetImpactInfo(int, + const idVec3* const point, impactInfo_t* const info) { + if (info == nullptr) return; + info->Zero(); + info->invMass = inverseMass; + info->position = point != nullptr + ? *point - current.i.position : kThreeDofZeroVector; + info->velocity = current.i.linearVelocity + + current.i.angularVelocity.Cross(info->position); +} +void idPhysics_RigidBody3DOF::ApplyImpulse(int, + const idVec3* const point, const idVec3* const impulse) { + if (flags.noImpact || impulse == nullptr) return; + current.i.linearVelocity = current.i.linearVelocity + + *impulse * inverseMass; + if (point != nullptr) + current.i.angularVelocity = current.i.angularVelocity + + (*point - current.i.position).Cross(*impulse) * inverseMass; + Activate(); +} +void idPhysics_RigidBody3DOF::ApplyForce(int, + const idVec3*, const idVec3* const force) { + if (force == nullptr) return; + current.externalForce = current.externalForce + *force * inverseMass; + Activate(); +} +void idPhysics_RigidBody3DOF::Activate() { + current.atRest = -1; + if (callbacks != nullptr) + GameLib_NotifyPhysicsActivated(callbacks, GetPhysicsId()); +} +void idPhysics_RigidBody3DOF::PutToRest() { + current.atRest = 1; + current.i.linearVelocity.Zero(); + current.i.angularVelocity.Zero(); + current.externalForce.Zero(); +} +bool idPhysics_RigidBody3DOF::IsAtRest() { return current.atRest >= 0; } +bool idPhysics_RigidBody3DOF::IsPushable(int) { return true; } +void idPhysics_RigidBody3DOF::SaveState() { saved = current; } +void idPhysics_RigidBody3DOF::RestoreState() { + current = saved; + LinkClip(); +} +void idPhysics_RigidBody3DOF::UpdateTime(int) {} + +void idPhysics_RigidBody3DOF::ClipTranslation(trace_t* const results, + const idVec3* const translation, const idClipModel* const model) { + idPhysics_DynamicBase::ClipTranslation(results, translation, + model != nullptr ? model : clipModel); +} +void idPhysics_RigidBody3DOF::ClipRotation(trace_t* const results, + const idRotation* const rotation, const idClipModel* const model) { + if (results == nullptr) return; + const idClipModel* moving = model != nullptr ? model : clipModel; + if (clip == nullptr || moving == nullptr || rotation == nullptr) { + std::memset(results, 0, sizeof(*results)); + results->fraction = 1.0f; + return; + } + clip->Rotation(results, current.i.position, *rotation, moving, + current.i.orientation, clipMask, GetEntityNumber(), false, + "idPhysics_RigidBody3DOF::ClipRotation"); +} +int idPhysics_RigidBody3DOF::ClipContents(const idClipModel* const model, + int queryMask) { + if (clip == nullptr || clipModel == nullptr) return 0; + if (queryMask == 0) queryMask = clipMask; + trace_t result{}; + if (model != nullptr) + clip->ContentsModel(result, current.i.position, clipModel, + current.i.orientation, queryMask, model->GetOrigin(), model, + model->GetAxis()); + else + clip->Contents(&result, current.i.position, clipModel, + current.i.orientation, queryMask, GetEntityNumber(), + "idPhysics_RigidBody3DOF::ClipContents"); + return result.c.contentFlags; +} +void idPhysics_RigidBody3DOF::DisableClip() { + if (clipModel != nullptr) clipModel->Disable(); +} +void idPhysics_RigidBody3DOF::EnableClip() { + if (clipModel != nullptr) clipModel->Enable(); +} +void idPhysics_RigidBody3DOF::UnlinkClip() { + if (clipModel != nullptr) clipModel->Unlink(); +} +void idPhysics_RigidBody3DOF::LinkClip() { + if (clipModel != nullptr) + clipModel->Link(GetEntityNumber(), GetEntityNumber(), + clipModel->GetBodyId(), current.i.position, + current.i.orientation); +} +bool idPhysics_RigidBody3DOF::EvaluateContacts() { + ClearContacts(); + AddGroundContacts(clipModel, 12); + return contacts.Num() != 0; +} +void idPhysics_RigidBody3DOF::SetPushed(const int deltaTime) { + if (deltaTime <= 0) { + current.linearPushVelocity.Zero(); + current.angularPushVelocity.Zero(); + return; + } + current.linearPushVelocity = (current.i.position - saved.i.position) + * (1000.0f / static_cast(deltaTime)); + current.angularPushVelocity = current.i.angularVelocity; +} +idVec3* idPhysics_RigidBody3DOF::GetPushedLinearVelocity( + idVec3* const result, int) { + if (result != nullptr) *result = current.linearPushVelocity; + return result; +} +idVec3* idPhysics_RigidBody3DOF::GetPushedAngularVelocity( + idVec3* const result, int) { + if (result != nullptr) *result = current.angularPushVelocity; + return result; +} + +void idPhysics_RigidBody3DOF::SetMaster(const bool enable, + const idVec3* const masterOrigin, const idMat3* const masterAxis, + const bindFlags_t bindFlags) { + translationQuery.index = 0; + if (enable && masterOrigin != nullptr && masterAxis != nullptr) { + if (!flags.hasMaster) { + current.localOrigin = masterAxis->Transpose() + * (current.i.position - *masterOrigin); + current.localAxis = (static_cast(bindFlags) & 1) != 0 + ? current.i.orientation * masterAxis->Transpose() + : current.i.orientation; + } + flags.hasMaster = 1; + flags.isOriented = (static_cast(bindFlags) & 1) != 0; + ClearContacts(); + } else if (flags.hasMaster) { + flags.hasMaster = 0; + Activate(); + } +} +void idPhysics_RigidBody3DOF::SetLocalOrigin(const idVec3* const origin, + int) { + if (origin == nullptr) return; + current.localOrigin = *origin; + if (flags.hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current.i.position = masterOrigin + masterAxis * *origin; + } else { + current.i.position = *origin; + } + LinkClip(); + Activate(); +} +void idPhysics_RigidBody3DOF::SetLocalAxis(const idMat3* const axis, int) { + if (axis == nullptr) return; + current.localAxis = *axis; + if (flags.hasMaster && flags.isOriented && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) current.i.orientation = *axis * masterAxis; + } else { + current.i.orientation = *axis; + } + LinkClip(); + Activate(); +} + +void idPhysics_RigidBody3DOF::Integrate(const float deltaTime, + const bodyState_t& source, bodyState_t& next) const { + next = source; + next.lastTimeStep = deltaTime; + next.i.position = source.i.position + + (source.i.linearVelocity + source.externalForce) * deltaTime; + idVec3 axis = source.i.angularVelocity; + const float speed = axis.NormalizeFast(); + const idRotation rotation(kThreeDofZeroVector, + speed > 0.0f ? axis : idVec3(0.0f, 0.0f, 1.0f), + speed * deltaTime * 57.29577951308232f); + next.i.orientation *= rotation.ToMat3(); + next.i.linearVelocity = source.i.linearVelocity + + gravityVector * deltaTime; + float linearDrag = linearFriction; + float angularDrag = angularFriction; + if (waterLevel > 0.0f) { + linearDrag += waterLevel * linearFrictionWater; + angularDrag += waterLevel * angularFrictionWater; + } + next.i.linearVelocity = next.i.linearVelocity + * (std::max)(0.0f, 1.0f - linearDrag * deltaTime); + next.i.angularVelocity = source.i.angularVelocity + * (std::max)(0.0f, 1.0f - angularDrag * deltaTime); +} + +bool idPhysics_RigidBody3DOF::CheckForCollisions( + const bodyState_t& source, bodyState_t& next, + trace_t& collision) { + if (clip == nullptr || clipModel == nullptr) return false; + idVec3 angularAxis = source.i.angularVelocity; + const float speed = angularAxis.NormalizeFast(); + const idRotation rotation(source.i.position, + speed > 0.0f ? angularAxis : idVec3(0.0f, 0.0f, 1.0f), + speed * next.lastTimeStep * 57.29577951308232f); + collision = {}; + collision.fraction = 1.0f; + collision.endpos = next.i.position; + collision.endAxis = next.i.orientation; + translationQuery = clip->Motion(&collision, source.i.position, + next.i.position, rotation, clipModel, source.i.orientation, + clipMask, GetEntityNumber(), false, + "idPhysics_RigidBody3DOF::CheckForCollisions"); + next.i.position = collision.endpos; + next.i.orientation = collision.endAxis; + return collision.fraction < 1.0f; +} + +bool idPhysics_RigidBody3DOF::CollisionImpulse( + const trace_t& collision, float, bodyState_t& state, + idVec3& impulse) { + const float normalSpeed = state.i.linearVelocity.Dot(collision.c.normal); + if (normalSpeed >= 0.0f) { + impulse.Zero(); + return false; + } + impulse = collision.c.normal + * (-(1.0f + coefficientOfRestitution) * normalSpeed * mass); + state.i.linearVelocity = state.i.linearVelocity + impulse * inverseMass; + const idVec3 tangent = state.i.linearVelocity + - collision.c.normal * state.i.linearVelocity.Dot(collision.c.normal); + state.i.linearVelocity = state.i.linearVelocity + - tangent * contactFriction; + return true; +} + +bool idPhysics_RigidBody3DOF::Evaluate(const int timeStepMSec, int) { + const float deltaTime = timeStepMSec * 0.001f; + current.lastTimeStep = deltaTime; + if (flags.hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (!GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) return false; + const idVec3 oldPosition = current.i.position; + current.i.position = masterOrigin + masterAxis * current.localOrigin; + current.i.orientation = flags.isOriented + ? current.localAxis * masterAxis : current.localAxis; + if (deltaTime > 0.0f) + current.i.linearVelocity = (current.i.position - oldPosition) + * (1.0f / deltaTime); + LinkClip(); + current.externalForce.Zero(); + return (current.i.position - oldPosition).LengthSqr() != 0.0f; + } + if (current.atRest >= 0 || deltaTime <= 0.0f) return false; + if (flags.dropToFloor) { + flags.dropToFloor = 0; + trace_t down{}; + const idVec3 end = current.i.position + gravityNormal * 128.0f; + clip->Translation(&down, current.i.position, end, clipModel, + current.i.orientation, clipMask, GetEntityNumber(), false, + "idPhysics_RigidBody3DOF::DropToFloor"); + current.i.position = down.endpos; + if (down.fraction < 1.0f) PutToRest(); + LinkClip(); + return true; + } + bodyState_t next; + Integrate(deltaTime, current, next); + trace_t collision{}; + const bool collided = CheckForCollisions(current, next, collision); + current = next; + if (collided) { + idVec3 impulse; + CollisionImpulse(collision, deltaTime, current, impulse); + ClearContacts(); + contacts.Append(collision.c); + UpdateCollisionResidency(collision.c); + AddContactPhysicsForContacts(); + if (callbacks != nullptr) + GameLib_NotifyPhysicsCollision(callbacks, GetPhysicsId(), + collision, current.i.linearVelocity); + const idVec3 alongGround = current.i.linearVelocity + - gravityNormal * current.i.linearVelocity.Dot(gravityNormal); + if (alongGround.Length() <= 10.0f + && std::fabs(current.i.linearVelocity.Dot(gravityNormal)) + < 20.0f) + PutToRest(); + } + LinkClip(); + current.linearPushVelocity.Zero(); + current.angularPushVelocity.Zero(); + current.externalForce.Zero(); + if (IsOutsideWorld() && callbacks != nullptr) + GameLib_NotifyPhysicsDeactivated(callbacks, GetPhysicsId()); + return true; +} + +int idPhysics_RigidBody3DOF::GetBlockingEntityNum() { + return contacts.Num() != 0 ? contacts[0].entityNum : 0x1FFF; +} +int idPhysics_RigidBody3DOF::GetLinearEndTime() { return 0; } +int idPhysics_RigidBody3DOF::GetAngularEndTime() { return 0; } +bool idPhysics_RigidBody3DOF::IsOutsideWorld() { + return idPhysics_DynamicBase::IsOutsideWorld(); +} diff --git a/source/engine/gamelib/physics/physics_rigidbody3dof.h b/source/engine/gamelib/physics/physics_rigidbody3dof.h index 35f2130..041838b 100644 --- a/source/engine/gamelib/physics/physics_rigidbody3dof.h +++ b/source/engine/gamelib/physics/physics_rigidbody3dof.h @@ -1,138 +1,132 @@ #pragma once -// Reconstructed C++ declarations from IDA Local Types and PDB/DIA metadata. -// Original PDB header: w:\tech5\engine\gamelib\physics\physics_rigidbody3dof.h -// Recovered logical types: 4 -// Signatures retain Xbox 360 ABI evidence and may still require manual review. +#include "gamelib/physics/clip.h" +#include "gamelib/physics/physics_dynamicbase.h" +#include "idlib/math/random.h" +#include -// IDA Local Type ordinal 17461; PDB kind: struct. -struct idPhysics_RigidBody3DOF::bodyIntState_t -{ - idVec3 position; - idMat3 orientation; - idVec3 linearVelocity; - idVec3 angularVelocity; -}; - -// IDA Local Type ordinal 17462; PDB kind: struct. -struct idPhysics_RigidBody3DOF::bodyState_t -{ - int atRest; - float lastTimeStep; - idVec3 localOrigin; - idMat3 localAxis; - idVec3 linearPushVelocity; - idVec3 angularPushVelocity; - idVec3 externalForce; - idPhysics_RigidBody3DOF::bodyIntState_t i; -}; - -// IDA Local Type ordinal 17463; PDB kind: struct. -struct idPhysics_RigidBody3DOF::simpleBodyFlags_t -{ - unsigned __int8 : 3; - __int8 noImpact : 1; - __int8 isOriented : 1; - __int8 hasMaster : 1; - __int8 testSolid : 1; - __int8 dropToFloor : 1; -}; - -// IDA Local Type ordinal 17464; PDB kind: class. -class __declspec(align(8)) idPhysics_RigidBody3DOF : public idPhysics_DynamicBase -{ +class idPhysics_RigidBody3DOF : public idPhysics_DynamicBase { public: - // Recovered virtual interface; IDA vtable ordinal 17465. - virtual ~idPhysics_RigidBody3DOF(); - virtual void SetClipModel(idClipModel *, float, int, bool); - virtual idClipModel *GetClipModel(int); - virtual int GetNumClipModels(); - virtual void SetMass(float, int); - virtual float GetMass(int); - virtual void SetContents(int, int); - virtual int GetContents(int); - virtual void SetClipMask(int, int); - virtual int GetClipMask(int); - virtual const idBounds *GetBounds(int); - virtual const idBounds *GetAbsBounds(int); - virtual void SetOrigin(const idVec3 *, int); - virtual void SetAxis(const idMat3 *, int); - virtual void Translate(const idVec3 *, int); - virtual void Rotate(const idRotation *, int); - virtual const idVec3 *GetOrigin(int); - virtual const idMat3 *GetAxis(int); - virtual const idVec3 *GetLocalOrigin(int); - virtual const idMat3 *GetLocalAxis(int); - virtual void SetLinearVelocity(const idVec3 *, int); - virtual void SetAngularVelocity(const idVec3 *, int); - virtual idVec3 *GetLinearVelocity(idVec3 *result, int); - virtual idVec3 *GetAngularVelocity(idVec3 *result, int); - virtual void SetGravity(const idVec3 *); - virtual const idVec3 *GetGravity(); - virtual const idVec3 *GetGravityNormal(); - virtual void SetWaterLevel(float, int); - virtual float GetWaterLevel(int); - virtual void SetWaterViscosity(float, int); - virtual float GetWaterViscosity(int); - virtual void SetWaterEntNum(int); - virtual int GetWaterEntNum(); - virtual void SetWaterSurfaceWrldHeight(float); - virtual float GetWaterSurfaceWrldHeight(); - virtual void GetImpactInfo(const int, const idVec3 *, impactInfo_t *); - virtual void ApplyImpulse(const int, const idVec3 *, const idVec3 *); - virtual void ApplyForce(const int, const idVec3 *, const idVec3 *); - virtual void Activate(); - virtual void PutToRest(); - virtual bool IsAtRest(); - virtual bool IsPushable(int); - virtual void SaveState(); - virtual void RestoreState(); - virtual bool Evaluate(int, int); - virtual void UpdateTime(int); - virtual void ClipTranslation(trace_t *, const idVec3 *, const idClipModel *); - virtual void ClipRotation(trace_t *, const idRotation *, const idClipModel *); - virtual int ClipContents(const idClipModel *, int); - virtual void DisableClip(); - virtual void EnableClip(); - virtual void UnlinkClip(); - virtual void LinkClip(); - virtual bool EvaluateContacts(); - virtual int GetNumContacts(); - virtual const contactInfo_t *GetContact(int); - virtual void ClearContacts(); - virtual void AddContactPhysics(idPhysics *); - virtual void RemoveContactPhysics(idPhysics *); - virtual int GetNumContactPhysics(); - virtual idPhysics *GetContactPhysics(int); - virtual void ActivateContactPhysics(); - virtual bool HasGroundContacts(); - virtual bool IsGroundEntity(int); - virtual bool IsGroundClipModel(int, int); - virtual void SetPushed(int); - virtual idVec3 *GetPushedLinearVelocity(idVec3 *result, const int); - virtual idVec3 *GetPushedAngularVelocity(idVec3 *result, const int); - virtual void SetMaster(bool, const idVec3 *, const idMat3 *, const bindFlags_t); - virtual void SetLocalOrigin(const idVec3 *, int); - virtual void SetLocalAxis(const idMat3 *, int); - virtual int GetBlockingEntityNum(); - virtual int GetLinearEndTime(); - virtual int GetAngularEndTime(); - virtual bool IsInNonResidentCollisionArea(bool); - virtual bool IsOutsideWorld(); + struct bodyIntState_t { + idVec3 position; + idMat3 orientation; + idVec3 linearVelocity; + idVec3 angularVelocity; + }; + struct bodyState_t { + int atRest; + float lastTimeStep; + idVec3 localOrigin; + idMat3 localAxis; + idVec3 linearPushVelocity; + idVec3 angularPushVelocity; + idVec3 externalForce; + bodyIntState_t i; - idClipModel *clipModel; - float mass; - float inverseMass; - float linearFriction; - float angularFriction; - float contactFriction; - float linearFrictionWater; - float angularFrictionWater; - float coefficientOfRestitution; - idPhysics_RigidBody3DOF::bodyState_t current; - idPhysics_RigidBody3DOF::bodyState_t saved; - idRandom random; - idClipQuery translationQuery; - idPhysics_RigidBody3DOF::simpleBodyFlags_t flags; + bodyState_t(); + }; + struct simpleBodyFlags_t { + std::uint8_t reserved : 3; + std::uint8_t noImpact : 1; + std::uint8_t isOriented : 1; + std::uint8_t hasMaster : 1; + std::uint8_t testSolid : 1; + std::uint8_t dropToFloor : 1; + }; + + idPhysics_RigidBody3DOF(); + ~idPhysics_RigidBody3DOF() override; + + void SetClipModel(idClipModel*, float, int, bool) override; + idClipModel* GetClipModel(int) override; + int GetNumClipModels() override; + void SetMass(float, int) override; + float GetMass(int) override; + void SetContents(int, int) override; + int GetContents(int) override; + const idBounds* GetBounds(int) override; + const idBounds* GetAbsBounds(int) override; + void SetOrigin(const idVec3*, int) override; + void SetAxis(const idMat3*, int) override; + void Translate(const idVec3*, int) override; + void Rotate(const idRotation*, int) override; + const idVec3* GetOrigin(int) override; + const idMat3* GetAxis(int) override; + const idVec3* GetLocalOrigin(int) override; + const idMat3* GetLocalAxis(int) override; + void SetLinearVelocity(const idVec3*, int) override; + void SetAngularVelocity(const idVec3*, int) override; + idVec3* GetLinearVelocity(idVec3*, int) override; + idVec3* GetAngularVelocity(idVec3*, int) override; + void SetWaterEntNum(int) override; + int GetWaterEntNum() override; + void SetWaterSurfaceWrldHeight(float) override; + float GetWaterSurfaceWrldHeight() override; + void GetImpactInfo(int, const idVec3*, impactInfo_t*) override; + void ApplyImpulse(int, const idVec3*, const idVec3*) override; + void ApplyForce(int, const idVec3*, const idVec3*) override; + void Activate() override; + void PutToRest() override; + bool IsAtRest() override; + bool IsPushable(int) override; + void SaveState() override; + void RestoreState() override; + bool Evaluate(int, int) override; + void UpdateTime(int) override; + void ClipTranslation(trace_t*, const idVec3*, const idClipModel*) override; + void ClipRotation(trace_t*, const idRotation*, const idClipModel*) override; + int ClipContents(const idClipModel*, int) override; + void DisableClip() override; + void EnableClip() override; + void UnlinkClip() override; + void LinkClip() override; + bool EvaluateContacts() override; + void SetPushed(int) override; + idVec3* GetPushedLinearVelocity(idVec3*, int) override; + idVec3* GetPushedAngularVelocity(idVec3*, int) override; + void SetMaster(bool, const idVec3*, const idMat3*, bindFlags_t) override; + void SetLocalOrigin(const idVec3*, int) override; + void SetLocalAxis(const idMat3*, int) override; + int GetBlockingEntityNum() override; + int GetLinearEndTime() override; + int GetAngularEndTime() override; + + bool IsOutsideWorld() override; + + void SetFriction(float linear, float angular, float contact); + void SetWaterFriction(float linear, float angular); + void SetCoefficientOfRestitution(float coefficient); + void Integrate(float deltaTime, const bodyState_t& current, + bodyState_t& next) const; + bool CheckForCollisions(const bodyState_t& current, + bodyState_t& next, trace_t& collision); + bool CollisionImpulse(const trace_t& collision, float timeStep, + bodyState_t& state, idVec3& impulse); + + idClipModel* clipModel; + float mass; + float inverseMass; + float linearFriction; + float angularFriction; + float contactFriction; + float linearFrictionWater; + float angularFrictionWater; + float coefficientOfRestitution; + bodyState_t current; + bodyState_t saved; + idRandom random; + idClipQuery translationQuery; + simpleBodyFlags_t flags; }; + +static_assert(sizeof(idPhysics_RigidBody3DOF::bodyIntState_t) == 72, + "Recovered 3DOF integration state ABI changed"); +static_assert(sizeof(idPhysics_RigidBody3DOF::bodyState_t) == 164, + "Recovered 3DOF body state ABI changed"); +static_assert(sizeof(idPhysics_RigidBody3DOF::simpleBodyFlags_t) == 1, + "Recovered 3DOF flags ABI changed"); +#if defined(_WIN32) && !defined(_WIN64) +static_assert(sizeof(idPhysics_RigidBody3DOF) == 504, + "Recovered idPhysics_RigidBody3DOF ABI changed"); +#endif diff --git a/source/engine/gamelib/physics/physics_staticmulti.cpp b/source/engine/gamelib/physics/physics_staticmulti.cpp new file mode 100644 index 0000000..beb9730 --- /dev/null +++ b/source/engine/gamelib/physics/physics_staticmulti.cpp @@ -0,0 +1,509 @@ +#include "gamelib/physics/physics_staticmulti.h" + +#include "gamelib/physics/clip.h" +#include "gamelib/physics/clipmodel.h" +#include "idlib/lib_print.h" +#include "idlib/math/rotation.h" + +#include +#include + +bool GameLib_GetMasterPhysicsTransform(idPhysicsCallbacks* callbacks, + idVec3& origin, idMat3& axis); + +namespace { + +const idVec3 kZeroVector(0.0f, 0.0f, 0.0f); +const idVec3 kDownVector(0.0f, 0.0f, -1.0f); +const idMat3 kIdentityAxis(1.0f); +const idBounds kZeroBounds{{idVec3(0.0f, 0.0f, 0.0f), + idVec3(0.0f, 0.0f, 0.0f)}}; + +staticPState_t DefaultState() { + staticPState_t state{}; + state.worldOrigin.Zero(); + state.worldAxis = idMat3(1.0f); + state.localOrigin.Zero(); + state.localAxis = idMat3(1.0f); + return state; +} + +void AddBounds(idBounds& destination, const idBounds& source, + bool& initialized) { + if (!initialized) { + destination = source; + initialized = true; + return; + } + for (int axis = 0; axis < 3; ++axis) { + destination[0][axis] = (std::min)(destination[0][axis], + source[0][axis]); + destination[1][axis] = (std::max)(destination[1][axis], + source[1][axis]); + } +} + +} // namespace + +idPhysics_StaticMulti::idPhysics_StaticMulti() + : idPhysics() + , clipModels(16) + , mass(16) + , bounds(kZeroBounds) + , absBounds(kZeroBounds) + , hasMaster(false) + , isOrientated(false) + , current(16) { + type = PHYSICS_STATICMULTI; + clipModels.SetNum(1); + mass.SetNum(1); + current.SetNum(1); + clipModels[0] = nullptr; + mass[0] = 1.0f; + current[0] = DefaultState(); +} + +idPhysics_StaticMulti::~idPhysics_StaticMulti() { + for (int index = 0; index < clipModels.Num(); ++index) { + if (clipModels[index] != nullptr) clipModels[index]->Delete(); + clipModels[index] = nullptr; + } +} + +bool idPhysics_StaticMulti::IsValidId(const int id) const { + return id >= 0 && id < clipModels.Num(); +} + +void idPhysics_StaticMulti::LinkModel(const int id) { + if (!IsValidId(id) || clipModels[id] == nullptr) return; + clipModels[id]->Link(GetEntityNumber(), GetEntityNumber(), id, + current[id].worldOrigin, current[id].worldAxis); +} + +void idPhysics_StaticMulti::SetClipModel(idClipModel* const model, + const float density, const int id, const bool freeOld) { + if (id < 0) return; + if (id >= clipModels.Num()) { + const int oldNum = clipModels.Num(); + const int newNum = id + 1; + if (!clipModels.SetNum(newNum) || !mass.SetNum(newNum) + || !current.SetNum(newNum)) return; + for (int index = oldNum; index < newNum; ++index) { + clipModels[index] = nullptr; + mass[index] = 1.0f; + current[index] = DefaultState(); + } + } + if (clipModels[id] != nullptr && clipModels[id] != model && freeOld) + clipModels[id]->Delete(); + clipModels[id] = model; + if (model != nullptr) { + if (model->GetNumTraceModels() != 0) { + idVec3 centerOfMass; + idMat3 inertia; + model->GetMassProperties(density, mass[id], centerOfMass, + inertia); + } else { + mass[id] = 1.0f; + } + LinkModel(id); + } + int newNum = clipModels.Num(); + while (newNum > 1 && clipModels[newNum - 1] == nullptr) --newNum; + clipModels.SetNum(newNum); + mass.SetNum(newNum); + current.SetNum(newNum); +} + +idClipModel* idPhysics_StaticMulti::GetClipModel(const int id) { + if (IsValidId(id) && clipModels[id] != nullptr) return clipModels[id]; + return clip != nullptr ? clip->defaultClipModel : nullptr; +} + +int idPhysics_StaticMulti::GetNumClipModels() { return clipModels.Num(); } + +void idPhysics_StaticMulti::SetMass(const float newMass, const int id) { + if (id == -1) { + if (mass.Num() == 0) return; + const float each = newMass / static_cast(mass.Num()); + for (int index = 0; index < mass.Num(); ++index) mass[index] = each; + } else if (IsValidId(id)) { + mass[id] = newMass; + } +} + +float idPhysics_StaticMulti::GetMass(const int id) { + if (id >= 0) return IsValidId(id) ? mass[id] : 0.0f; + float total = 0.0f; + for (int index = 0; index < mass.Num(); ++index) total += mass[index]; + return total; +} + +void idPhysics_StaticMulti::SetContents(const int contents, const int id) { + if (id == -1) { + for (int index = 0; index < clipModels.Num(); ++index) + if (clipModels[index] != nullptr) + clipModels[index]->SetContents(contents); + } else if (IsValidId(id) && clipModels[id] != nullptr) { + clipModels[id]->SetContents(contents); + } +} + +int idPhysics_StaticMulti::GetContents(const int id) { + if (id >= 0) return IsValidId(id) && clipModels[id] != nullptr + ? clipModels[id]->GetContents() : 0; + int contents = 0; + for (int index = 0; index < clipModels.Num(); ++index) + if (clipModels[index] != nullptr) + contents |= clipModels[index]->GetContents(); + return contents; +} + +void idPhysics_StaticMulti::SetClipMask(int, int) {} +int idPhysics_StaticMulti::GetClipMask(int) { return 0; } + +const idBounds* idPhysics_StaticMulti::GetBounds(const int id) { + if (IsValidId(id) && clipModels[id] != nullptr) + return &clipModels[id]->GetBounds(); + if (id != -1) return &kZeroBounds; + bool initialized = false; + for (int index = 0; index < clipModels.Num(); ++index) + if (clipModels[index] != nullptr) + AddBounds(bounds, clipModels[index]->GetBounds(), initialized); + if (!initialized) return &kZeroBounds; + const idVec3 origin = current.Num() != 0 + ? current[0].worldOrigin : kZeroVector; + bounds[0] = bounds[0] - origin; + bounds[1] = bounds[1] - origin; + return &bounds; +} + +const idBounds* idPhysics_StaticMulti::GetAbsBounds(const int id) { + if (IsValidId(id) && clipModels[id] != nullptr) + return &clipModels[id]->GetAbsBounds(); + if (id != -1) return &kZeroBounds; + bool initialized = false; + for (int index = 0; index < clipModels.Num(); ++index) + if (clipModels[index] != nullptr) + AddBounds(absBounds, clipModels[index]->GetAbsBounds(), + initialized); + return initialized ? &absBounds : &kZeroBounds; +} + +bool idPhysics_StaticMulti::Evaluate(int, int) { + if (!hasMaster || callbacks == nullptr) return false; + idVec3 masterOrigin; + idMat3 masterAxis; + if (!GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) return false; + bool moved = false; + for (int index = 0; index < current.Num(); ++index) { + const idVec3 oldOrigin = current[index].worldOrigin; + const idMat3 oldAxis = current[index].worldAxis; + current[index].worldOrigin = masterOrigin + + masterAxis * current[index].localOrigin; + current[index].worldAxis = isOrientated + ? current[index].localAxis * masterAxis + : current[index].localAxis; + moved = moved + || (current[index].worldOrigin - oldOrigin).LengthSqr() != 0.0f + || (current[index].worldAxis[0] - oldAxis[0]).LengthSqr() != 0.0f + || (current[index].worldAxis[1] - oldAxis[1]).LengthSqr() != 0.0f + || (current[index].worldAxis[2] - oldAxis[2]).LengthSqr() != 0.0f; + LinkModel(index); + } + return moved; +} + +void idPhysics_StaticMulti::SetOrigin(const idVec3* const newOrigin, + const int id) { + if (newOrigin == nullptr || current.Num() == 0) return; + if (id == -1) { + const idVec3 translation = *newOrigin - current[0].worldOrigin; + Translate(&translation, -1); + return; + } + if (!IsValidId(id)) return; + current[id].worldOrigin = *newOrigin; + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current[id].localOrigin = masterAxis.Transpose() + * (*newOrigin - masterOrigin); + } else { + current[id].localOrigin = *newOrigin; + } + LinkModel(id); +} + +void idPhysics_StaticMulti::SetAxis(const idMat3* const newAxis, + const int id) { + if (newAxis == nullptr || current.Num() == 0) return; + if (id == -1) { + const idMat3 delta = current[0].worldAxis.Transpose() * *newAxis; + const idVec3 pivot = current[0].worldOrigin; + for (int index = 0; index < current.Num(); ++index) { + current[index].worldOrigin = pivot + + delta * (current[index].worldOrigin - pivot); + current[index].worldAxis *= delta; + current[index].localAxis *= delta; + LinkModel(index); + } + return; + } + if (!IsValidId(id)) return; + current[id].worldAxis = *newAxis; + if (hasMaster && isOrientated && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current[id].localAxis = *newAxis * masterAxis.Transpose(); + } else { + current[id].localAxis = *newAxis; + } + LinkModel(id); +} + +void idPhysics_StaticMulti::Translate(const idVec3* const translation, + const int id) { + if (translation == nullptr) return; + const int first = id == -1 ? 0 : id; + const int last = id == -1 ? current.Num() : id + 1; + if (first < 0 || last > current.Num()) return; + for (int index = first; index < last; ++index) { + current[index].worldOrigin = current[index].worldOrigin + *translation; + current[index].localOrigin = current[index].localOrigin + *translation; + LinkModel(index); + } +} + +void idPhysics_StaticMulti::Rotate(const idRotation* const rotation, + const int id) { + if (rotation == nullptr) return; + const int first = id == -1 ? 0 : id; + const int last = id == -1 ? current.Num() : id + 1; + if (first < 0 || last > current.Num()) return; + const idMat3 rotationAxis = rotation->ToMat3(); + for (int index = first; index < last; ++index) { + current[index].worldOrigin = *rotation * current[index].worldOrigin; + current[index].worldAxis *= rotationAxis; + current[index].localOrigin = *rotation * current[index].localOrigin; + current[index].localAxis *= rotationAxis; + LinkModel(index); + } +} + +const idVec3* idPhysics_StaticMulti::GetOrigin(const int id) { + return IsValidId(id) ? ¤t[id].worldOrigin : &kZeroVector; +} +const idMat3* idPhysics_StaticMulti::GetAxis(const int id) { + return IsValidId(id) ? ¤t[id].worldAxis : &kIdentityAxis; +} +const idVec3* idPhysics_StaticMulti::GetLocalOrigin(const int id) { + return IsValidId(id) ? ¤t[id].localOrigin : &kZeroVector; +} +const idMat3* idPhysics_StaticMulti::GetLocalAxis(const int id) { + return IsValidId(id) ? ¤t[id].localAxis : &kIdentityAxis; +} + +void idPhysics_StaticMulti::SetLinearVelocity(const idVec3*, int) {} +void idPhysics_StaticMulti::SetAngularVelocity(const idVec3*, int) {} +idVec3* idPhysics_StaticMulti::GetLinearVelocity(idVec3* result, int) { + if (result != nullptr) result->Zero(); + return result; +} +idVec3* idPhysics_StaticMulti::GetAngularVelocity(idVec3* result, int) { + if (result != nullptr) result->Zero(); + return result; +} +void idPhysics_StaticMulti::SetGravity(const idVec3*) {} +const idVec3* idPhysics_StaticMulti::GetGravity() { return &kZeroVector; } +const idVec3* idPhysics_StaticMulti::GetGravityNormal() { + return &kDownVector; +} +void idPhysics_StaticMulti::SetWaterLevel(float, int) {} +float idPhysics_StaticMulti::GetWaterLevel(int) { return 0.0f; } +void idPhysics_StaticMulti::SetWaterViscosity(float, int) {} +float idPhysics_StaticMulti::GetWaterViscosity(int) { return 0.0f; } +void idPhysics_StaticMulti::SetWaterEntNum(int) {} +int idPhysics_StaticMulti::GetWaterEntNum() { return -1; } +void idPhysics_StaticMulti::SetWaterSurfaceWrldHeight(float) {} +float idPhysics_StaticMulti::GetWaterSurfaceWrldHeight() { return 0.0f; } +void idPhysics_StaticMulti::GetImpactInfo(int, const idVec3*, + impactInfo_t* const info) { + if (info != nullptr) info->Zero(); +} +void idPhysics_StaticMulti::ApplyImpulse(int, const idVec3*, const idVec3*) {} +void idPhysics_StaticMulti::ApplyForce(int, const idVec3*, const idVec3*) {} +void idPhysics_StaticMulti::Activate() {} +void idPhysics_StaticMulti::PutToRest() {} +bool idPhysics_StaticMulti::IsAtRest() { return true; } +bool idPhysics_StaticMulti::IsPushable(int) { return false; } +void idPhysics_StaticMulti::SaveState() {} +void idPhysics_StaticMulti::RestoreState() {} +void idPhysics_StaticMulti::UpdateTime(int) {} + +void idPhysics_StaticMulti::ClipTranslation(trace_t* const results, + const idVec3*, const idClipModel*) { + if (results != nullptr) std::memset(results, 0, sizeof(*results)); + idLibPrint::Warning("idPhysics_StaticMulti::ClipTranslation called"); +} + +void idPhysics_StaticMulti::ClipRotation(trace_t* const results, + const idRotation*, const idClipModel*) { + if (results != nullptr) std::memset(results, 0, sizeof(*results)); + idLibPrint::Warning("idPhysics_StaticMulti::ClipRotation called"); +} + +int idPhysics_StaticMulti::ClipContents(const idClipModel* const model, + int clipMask) { + if (clip == nullptr) return 0; + if (clipMask == 0) clipMask = -1; + int contents = 0; + for (int index = 0; index < clipModels.Num(); ++index) { + idClipModel* self = clipModels[index]; + if (self == nullptr) continue; + trace_t result{}; + if (model != nullptr) { + clip->ContentsModel(result, self->GetOrigin(), self, + self->GetAxis(), clipMask, model->GetOrigin(), model, + model->GetAxis()); + } else { + clip->Contents(&result, self->GetOrigin(), self, + self->GetAxis(), clipMask, 0x1FFF, + "idPhysics_StaticMulti::ClipContents"); + } + contents |= result.c.contentFlags; + } + return contents; +} + +void idPhysics_StaticMulti::DisableClip() { + for (int index = 0; index < clipModels.Num(); ++index) + if (clipModels[index] != nullptr) clipModels[index]->Disable(); +} +void idPhysics_StaticMulti::EnableClip() { + for (int index = 0; index < clipModels.Num(); ++index) + if (clipModels[index] != nullptr) clipModels[index]->Enable(); +} +void idPhysics_StaticMulti::UnlinkClip() { + for (int index = 0; index < clipModels.Num(); ++index) + if (clipModels[index] != nullptr) clipModels[index]->Unlink(); +} +void idPhysics_StaticMulti::LinkClip() { + for (int index = 0; index < clipModels.Num(); ++index) LinkModel(index); +} +bool idPhysics_StaticMulti::EvaluateContacts() { return false; } +int idPhysics_StaticMulti::GetNumContacts() { return 0; } +const contactInfo_t* idPhysics_StaticMulti::GetContact(int) { + static contactInfo_t info{}; + std::memset(&info, 0, sizeof(info)); + return &info; +} +void idPhysics_StaticMulti::ClearContacts() {} +void idPhysics_StaticMulti::AddContactPhysics(idPhysics*) {} +void idPhysics_StaticMulti::RemoveContactPhysics(idPhysics*) {} +int idPhysics_StaticMulti::GetNumContactPhysics() { return 0; } +idPhysics* idPhysics_StaticMulti::GetContactPhysics(int) { return nullptr; } +void idPhysics_StaticMulti::ActivateContactPhysics() {} +bool idPhysics_StaticMulti::HasGroundContacts() { return false; } +bool idPhysics_StaticMulti::IsGroundEntity(int) { return false; } +bool idPhysics_StaticMulti::IsGroundClipModel(int, int) { return false; } +void idPhysics_StaticMulti::SetPushed(int) {} +idVec3* idPhysics_StaticMulti::GetPushedLinearVelocity(idVec3* result, int) { + if (result != nullptr) result->Zero(); + return result; +} +idVec3* idPhysics_StaticMulti::GetPushedAngularVelocity(idVec3* result, int) { + if (result != nullptr) result->Zero(); + return result; +} + +void idPhysics_StaticMulti::SetMaster(const bool enable, + const idVec3* const masterOrigin, const idMat3* const masterAxis, + const bindFlags_t flags) { + if (enable && masterOrigin != nullptr && masterAxis != nullptr) { + if (!hasMaster) { + const idMat3 inverse = masterAxis->Transpose(); + for (int index = 0; index < current.Num(); ++index) { + current[index].localOrigin = inverse + * (current[index].worldOrigin - *masterOrigin); + current[index].localAxis = + (static_cast(flags) & 1) != 0 + ? current[index].worldAxis * inverse + : current[index].worldAxis; + } + } + hasMaster = true; + isOrientated = (static_cast(flags) & 1) != 0; + } else { + hasMaster = false; + } +} + +void idPhysics_StaticMulti::SetLocalOrigin(const idVec3* const newOrigin, + const int id) { + if (newOrigin == nullptr || current.Num() == 0) return; + if (id == -1) { + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) { + const idVec3 target = masterOrigin + masterAxis * *newOrigin; + const idVec3 translation = target - current[0].worldOrigin; + Translate(&translation, -1); + } + } else { + const idVec3 translation = *newOrigin - current[0].worldOrigin; + Translate(&translation, -1); + } + return; + } + if (!IsValidId(id)) return; + current[id].localOrigin = *newOrigin; + if (hasMaster && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) + current[id].worldOrigin = masterOrigin + masterAxis * *newOrigin; + } else { + current[id].worldOrigin = *newOrigin; + } + LinkModel(id); +} + +void idPhysics_StaticMulti::SetLocalAxis(const idMat3* const newAxis, + const int id) { + if (newAxis == nullptr || current.Num() == 0) return; + if (id == -1) { + idMat3 target = *newAxis; + if (hasMaster && isOrientated && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) target *= masterAxis; + } + SetAxis(&target, -1); + return; + } + if (!IsValidId(id)) return; + current[id].localAxis = *newAxis; + if (hasMaster && isOrientated && callbacks != nullptr) { + idVec3 masterOrigin; + idMat3 masterAxis; + if (GameLib_GetMasterPhysicsTransform(callbacks, masterOrigin, + masterAxis)) current[id].worldAxis = *newAxis * masterAxis; + } else { + current[id].worldAxis = *newAxis; + } + LinkModel(id); +} + +int idPhysics_StaticMulti::GetBlockingEntityNum() { return 0x1FFF; } +int idPhysics_StaticMulti::GetLinearEndTime() { return 0; } +int idPhysics_StaticMulti::GetAngularEndTime() { return 0; } diff --git a/source/engine/gamelib/physics/physics_staticmulti.h b/source/engine/gamelib/physics/physics_staticmulti.h index 23703d4..388291e 100644 --- a/source/engine/gamelib/physics/physics_staticmulti.h +++ b/source/engine/gamelib/physics/physics_staticmulti.h @@ -1,97 +1,100 @@ #pragma once -// Reconstructed C++ declarations from IDA Local Types and PDB/DIA metadata. -// Original PDB header: w:\tech5\engine\gamelib\physics\physics_staticmulti.h -// Recovered logical types: 1 -// Signatures retain Xbox 360 ABI evidence and may still require manual review. +#include "gamelib/physics/physics_static.h" - -// IDA Local Type ordinal 18829; PDB kind: class. -class __declspec(align(8)) idPhysics_StaticMulti : public idPhysics -{ +class idPhysics_StaticMulti : public idPhysics { public: - // Recovered virtual interface; IDA vtable ordinal 18830. - virtual ~idPhysics_StaticMulti(); - virtual void SetClipModel(idClipModel *, float, int, bool); - virtual idClipModel *GetClipModel(int); - virtual int GetNumClipModels(); - virtual void SetMass(float, int); - virtual float GetMass(int); - virtual void SetContents(int, int); - virtual int GetContents(int); - virtual void SetClipMask(int, int); - virtual int GetClipMask(int); - virtual const idBounds *GetBounds(int); - virtual const idBounds *GetAbsBounds(int); - virtual void SetOrigin(const idVec3 *, int); - virtual void SetAxis(const idMat3 *, int); - virtual void Translate(const idVec3 *, int); - virtual void Rotate(const idRotation *, int); - virtual const idVec3 *GetOrigin(int); - virtual const idMat3 *GetAxis(int); - virtual const idVec3 *GetLocalOrigin(int); - virtual const idMat3 *GetLocalAxis(int); - virtual void SetLinearVelocity(const idVec3 *, int); - virtual void SetAngularVelocity(const idVec3 *, int); - virtual idVec3 *GetLinearVelocity(idVec3 *result, int); - virtual idVec3 *GetAngularVelocity(idVec3 *result, int); - virtual void SetGravity(const idVec3 *); - virtual const idVec3 *GetGravity(); - virtual const idVec3 *GetGravityNormal(); - virtual void SetWaterLevel(float, int); - virtual float GetWaterLevel(int); - virtual void SetWaterViscosity(float, int); - virtual float GetWaterViscosity(int); - virtual void SetWaterEntNum(int); - virtual int GetWaterEntNum(); - virtual void SetWaterSurfaceWrldHeight(float); - virtual float GetWaterSurfaceWrldHeight(); - virtual void GetImpactInfo(const int, const idVec3 *, impactInfo_t *); - virtual void ApplyImpulse(const int, const idVec3 *, const idVec3 *); - virtual void ApplyForce(const int, const idVec3 *, const idVec3 *); - virtual void Activate(); - virtual void PutToRest(); - virtual bool IsAtRest(); - virtual bool IsPushable(int); - virtual void SaveState(); - virtual void RestoreState(); - virtual bool Evaluate(int, int); - virtual void UpdateTime(int); - virtual void ClipTranslation(trace_t *, const idVec3 *, const idClipModel *); - virtual void ClipRotation(trace_t *, const idRotation *, const idClipModel *); - virtual int ClipContents(const idClipModel *, int); - virtual void DisableClip(); - virtual void EnableClip(); - virtual void UnlinkClip(); - virtual void LinkClip(); - virtual bool EvaluateContacts(); - virtual int GetNumContacts(); - virtual const contactInfo_t *GetContact(int); - virtual void ClearContacts(); - virtual void AddContactPhysics(idPhysics *); - virtual void RemoveContactPhysics(idPhysics *); - virtual int GetNumContactPhysics(); - virtual idPhysics *GetContactPhysics(int); - virtual void ActivateContactPhysics(); - virtual bool HasGroundContacts(); - virtual bool IsGroundEntity(int); - virtual bool IsGroundClipModel(int, int); - virtual void SetPushed(int); - virtual idVec3 *GetPushedLinearVelocity(idVec3 *result, const int); - virtual idVec3 *GetPushedAngularVelocity(idVec3 *result, const int); - virtual void SetMaster(bool, const idVec3 *, const idMat3 *, const bindFlags_t); - virtual void SetLocalOrigin(const idVec3 *, int); - virtual void SetLocalAxis(const idMat3 *, int); - virtual int GetBlockingEntityNum(); - virtual int GetLinearEndTime(); - virtual int GetAngularEndTime(); - virtual bool IsInNonResidentCollisionArea(bool); + idPhysics_StaticMulti(); + ~idPhysics_StaticMulti() override; - idList clipModels; - idList mass; - idBounds bounds; - idBounds absBounds; - bool hasMaster; - bool isOrientated; - idList current; + void SetClipModel(idClipModel*, float, int, bool) override; + idClipModel* GetClipModel(int) override; + int GetNumClipModels() override; + void SetMass(float, int) override; + float GetMass(int) override; + void SetContents(int, int) override; + int GetContents(int) override; + void SetClipMask(int, int) override; + int GetClipMask(int) override; + const idBounds* GetBounds(int) override; + const idBounds* GetAbsBounds(int) override; + void SetOrigin(const idVec3*, int) override; + void SetAxis(const idMat3*, int) override; + void Translate(const idVec3*, int) override; + void Rotate(const idRotation*, int) override; + const idVec3* GetOrigin(int) override; + const idMat3* GetAxis(int) override; + const idVec3* GetLocalOrigin(int) override; + const idMat3* GetLocalAxis(int) override; + void SetLinearVelocity(const idVec3*, int) override; + void SetAngularVelocity(const idVec3*, int) override; + idVec3* GetLinearVelocity(idVec3*, int) override; + idVec3* GetAngularVelocity(idVec3*, int) override; + void SetGravity(const idVec3*) override; + const idVec3* GetGravity() override; + const idVec3* GetGravityNormal() override; + void SetWaterLevel(float, int) override; + float GetWaterLevel(int) override; + void SetWaterViscosity(float, int) override; + float GetWaterViscosity(int) override; + void SetWaterEntNum(int) override; + int GetWaterEntNum() override; + void SetWaterSurfaceWrldHeight(float) override; + float GetWaterSurfaceWrldHeight() override; + void GetImpactInfo(int, const idVec3*, impactInfo_t*) override; + void ApplyImpulse(int, const idVec3*, const idVec3*) override; + void ApplyForce(int, const idVec3*, const idVec3*) override; + void Activate() override; + void PutToRest() override; + bool IsAtRest() override; + bool IsPushable(int) override; + void SaveState() override; + void RestoreState() override; + bool Evaluate(int, int) override; + void UpdateTime(int) override; + void ClipTranslation(trace_t*, const idVec3*, const idClipModel*) override; + void ClipRotation(trace_t*, const idRotation*, const idClipModel*) override; + int ClipContents(const idClipModel*, int) override; + void DisableClip() override; + void EnableClip() override; + void UnlinkClip() override; + void LinkClip() override; + bool EvaluateContacts() override; + int GetNumContacts() override; + const contactInfo_t* GetContact(int) override; + void ClearContacts() override; + void AddContactPhysics(idPhysics*) override; + void RemoveContactPhysics(idPhysics*) override; + int GetNumContactPhysics() override; + idPhysics* GetContactPhysics(int) override; + void ActivateContactPhysics() override; + bool HasGroundContacts() override; + bool IsGroundEntity(int) override; + bool IsGroundClipModel(int, int) override; + void SetPushed(int) override; + idVec3* GetPushedLinearVelocity(idVec3*, int) override; + idVec3* GetPushedAngularVelocity(idVec3*, int) override; + void SetMaster(bool, const idVec3*, const idMat3*, bindFlags_t) override; + void SetLocalOrigin(const idVec3*, int) override; + void SetLocalAxis(const idMat3*, int) override; + int GetBlockingEntityNum() override; + int GetLinearEndTime() override; + int GetAngularEndTime() override; + + idList clipModels; + idList mass; + idBounds bounds; + idBounds absBounds; + bool hasMaster; + bool isOrientated; + idList current; + +private: + bool IsValidId(int id) const; + void LinkModel(int id); }; + +#if defined(_WIN32) && !defined(_WIN64) +static_assert(sizeof(idPhysics_StaticMulti) == 152, + "Recovered idPhysics_StaticMulti ABI changed"); +#endif