Justin Marshall
1282 lines
49 KiB
C++
1282 lines
49 KiB
C++
#include "gamelib/physics/physics_af.h"
|
|
|
|
#include "gamelib/physics/clipmodel.h"
|
|
#include "idlib/math/rotation.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
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<idAFBody*, 71>& 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<idAFBody*, 71>& 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<idAFConstraint*, 71>& 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<float>(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<idAFBody*>(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<idAFConstraint*, 71> 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<float>(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<bindFlags_t>(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;
|
|
}
|
|
|