Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add warm starting #1389

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Add warm starting #1389

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
89 changes: 85 additions & 4 deletions engine/src/physics/solver/penetration_constraint/plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
using namespace cubos::engine;

CUBOS_DEFINE_TAG(cubos::engine::addPenetrationConstraintTag);
CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintWarmStartTag);
CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintSolveTag);
CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintSolveRelaxTag);
CUBOS_DEFINE_TAG(cubos::engine::penetrationConstraintRestitutionTag);
Expand Down Expand Up @@ -230,11 +231,68 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
cubos.relation<PenetrationConstraint>();

cubos.tag(addPenetrationConstraintTag);
cubos.tag(penetrationConstraintWarmStartTag);
cubos.tag(penetrationConstraintSolveTag);
cubos.tag(penetrationConstraintSolveRelaxTag);
cubos.tag(penetrationConstraintRestitutionTag);
cubos.tag(penetrationConstraintCleanTag);

cubos.system("warm start")
.tagged(penetrationConstraintWarmStartTag)
.after(addPenetrationConstraintTag)
.before(penetrationConstraintSolveTag)
.tagged(fixedSubstepTag)
.call([](Query<Entity, const Mass&, const Inertia&, const Rotation&, AccumulatedCorrection&, Velocity&,
AngularVelocity&, PenetrationConstraint&, Entity, const Mass&, const Inertia&, const Rotation&,
AccumulatedCorrection&, Velocity&, AngularVelocity&>
query) {
for (auto [ent1, mass1, inertia1, rotation1, correction1, velocity1, angVelocity1, constraint, ent2, mass2,
inertia2, rotation2, correction2, velocity2, angVelocity2] : query)
{
glm::vec3 v1 = velocity1.vec;
glm::vec3 v2 = velocity2.vec;

glm::vec3 w1 = angVelocity1.vec;
glm::vec3 w2 = angVelocity2.vec;

glm::vec3 tangent1;
glm::vec3 tangent2;
if (ent1 != constraint.entity)
{
getTangents(v2, v1, constraint.normal, tangent1, tangent2);
}
else
{
getTangents(v1, v2, constraint.normal, tangent1, tangent2);
}

for (PenetrationConstraintPointData& contactPoint : constraint.points)
{
glm::vec3 r1 = contactPoint.fixedAnchor1;
glm::vec3 r2 = contactPoint.fixedAnchor2;

/// TODO: replace this number by a warmStartCoefficient
glm::vec3 p = 1.0F * (contactPoint.normalImpulse * constraint.normal) +
(contactPoint.frictionImpulse1 * tangent1) +
(contactPoint.frictionImpulse2 * tangent2);
if (ent1 != constraint.entity)
{
p *= -1.0F;
}

v1 -= p * mass1.inverseMass;
w1 -= inertia1.inverseInertia * glm::cross(r1, p);
v2 += p * mass2.inverseMass;
w2 += inertia2.inverseInertia * glm::cross(r2, p);
}

velocity1.vec = v1;
angVelocity1.vec = w1;
velocity2.vec = v2;
angVelocity2.vec = w2;
}
});

cubos.system("solve contacts bias")
.tagged(penetrationConstraintSolveTag)
.tagged(physicsSolveContactTag)
Expand Down Expand Up @@ -353,10 +411,9 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
{
auto pointData = PenetrationConstraintPointData{};

/// TODO: when we have warm-start change this
pointData.normalImpulse = 0.0F;
pointData.frictionImpulse1 = 0.0F;
pointData.frictionImpulse2 = 0.0F;
pointData.normalImpulse = point.normalImpulse;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ clang-diagnostic-error ⚠️
no member named normalImpulse in cubos::engine::ContactPointData

pointData.frictionImpulse1 = point.frictionImpulse1;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ clang-diagnostic-error ⚠️
no member named frictionImpulse1 in cubos::engine::ContactPointData

pointData.frictionImpulse2 = point.frictionImpulse2;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ clang-diagnostic-error ⚠️
no member named frictionImpulse2 in cubos::engine::ContactPointData


pointData.localAnchor1 = point.localOn1 - centerOfMass1.vec;
pointData.localAnchor2 = point.localOn2 - centerOfMass2.vec;
Expand Down Expand Up @@ -441,6 +498,29 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
}
});

/// TODO: create tag for this
cubos.system("store impulses and clean penetration constraint pairs")
.tagged(physicsFinalizePositionTag)
.call([](Commands cmds, Query<Entity, ContactManifold&, Entity> cQuery,
Query<Entity, PenetrationConstraint&, Entity> pQuery) {
for (auto [entity, constraint, other] : cQuery)
{
if (auto match = pQuery.pin(0, entity).pin(1, other).first())
{
auto [entity, manifold, other] = *match;
// CUBOS_DEBUG("msize: {}, csize: {}", manifold.points.size(), constraint.points.size());
for (int i = 0; i < manifold.points.size(); i++)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ clang-diagnostic-sign-compare ⚠️
comparison of integers of different signs: int and std::vector::size_type (aka unsigned long)

{
manifold.points[i].normalImpulse = constraint.points[i].normalImpulse;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ clang-diagnostic-error ⚠️
no member named normalImpulse in cubos::engine::ContactPointData

manifold.points[i].frictionImpulse1 = constraint.points[i].frictionImpulse1;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ clang-diagnostic-error ⚠️
no member named frictionImpulse1 in cubos::engine::ContactPointData

manifold.points[i].frictionImpulse2 = constraint.points[i].frictionImpulse2;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ clang-diagnostic-error ⚠️
no member named frictionImpulse2 in cubos::engine::ContactPointData

}
}
cmds.unrelate<PenetrationConstraint>(entity, other);
}
});

/*
cubos.system("clean penetration constraint pairs")
.tagged(penetrationConstraintCleanTag)
.before(addPenetrationConstraintTag)
Expand All @@ -451,4 +531,5 @@ void cubos::engine::penetrationConstraintPlugin(Cubos& cubos)
cmds.unrelate<PenetrationConstraint>(entity, other);
}
});
*/
}
1 change: 1 addition & 0 deletions engine/src/physics/solver/penetration_constraint/plugin.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ namespace cubos::engine
/// @brief Adds solver for penetration constraint.

extern Tag addPenetrationConstraintTag;
extern Tag penetrationConstraintWarmStartTag;
extern Tag penetrationConstraintCleanTag;
extern Tag penetrationConstraintSolveTag;
extern Tag penetrationConstraintRestitutionTag;
Expand Down
Loading