68 #include <type_traits>
72 #include <unordered_map>
79 #if defined(DO_THREADED)
83 #define PLAYRHO_MAGIC(x) (x)
118 const auto translation = h * velocity.
linear;
119 const auto rotation = h * velocity.angular;
135 const auto numContacts =
size(contacts);
136 for (
auto i = decltype(numContacts){0}; i < numContacts; ++i)
155 for (
auto i = decltype(count){0}; i < count; ++i)
164 for_each(cbegin(velConstraints), cend(velConstraints),
178 map.reserve(
size(bodies));
179 for_each(cbegin(bodies), cend(bodies), [&](
const BodyPtr& body) {
180 const auto i =
static_cast<size_t>(&body - data(bodies));
181 assert(i <
size(bodies));
182 #ifdef USE_VECTOR_MAP
185 map[body] = &bodyConstraints[i];
188 #ifdef USE_VECTOR_MAP
190 return std::get<const Body*>(a) < std::get<const Body*>(b);
199 constraints.reserve(
size(bodies));
200 transform(cbegin(bodies), cend(bodies), back_inserter(constraints), [&](
const BodyPtr &b) {
210 constraints.reserve(
size(contacts));
211 transform(cbegin(contacts), cend(contacts), back_inserter(constraints), [&](
const Contact *contact) {
219 const auto bodyA =
GetBodyA(*contact);
220 const auto shapeA = fixtureA.GetShape();
222 const auto bodyB =
GetBodyB(*contact);
223 const auto shapeB = fixtureB.GetShape();
225 const auto bodyConstraintA =
At(bodies, bodyA);
226 const auto bodyConstraintB =
At(bodies, bodyB);
232 manifold, *bodyConstraintA, radiusA, *bodyConstraintB, radiusB
250 velConstraints.reserve(
size(contacts));
251 transform(cbegin(contacts), cend(contacts), back_inserter(velConstraints), [&](
const ContactPtr& contact) {
261 const auto bodyA = fixtureA->GetBody();
262 const auto shapeA = fixtureA->GetShape();
264 const auto bodyB = fixtureB->GetBody();
265 const auto shapeB = fixtureB->GetShape();
267 const auto bodyConstraintA =
At(bodies, bodyA);
268 const auto bodyConstraintB =
At(bodies, bodyB);
274 bodyConstraintA->GetLocalCenter());
276 bodyConstraintB->GetLocalCenter());
277 const auto worldManifold =
GetWorldManifold(manifold, xfA, radiusA, xfB, radiusB);
280 *bodyConstraintA, *bodyConstraintB, conf};
282 return velConstraints;
292 auto maxIncImpulse = 0_Ns;
297 return maxIncImpulse;
309 auto minSeparation = std::numeric_limits<Length>::infinity();
316 minSeparation = std::min(minSeparation, res.min_separation);
319 return minSeparation;
343 auto minSeparation = std::numeric_limits<Length>::infinity();
346 const auto moveA = (pc.
GetBodyA() == bodyConstraintA) || (pc.
GetBodyA() == bodyConstraintB);
347 const auto moveB = (pc.
GetBodyB() == bodyConstraintA) || (pc.
GetBodyB() == bodyConstraintB);
351 minSeparation = std::min(minSeparation, res.min_separation);
354 return minSeparation;
358 inline Time GetUnderActiveTime(
const Body& b,
const StepConf& conf) noexcept
368 auto minUnderActiveTime = std::numeric_limits<Time>::infinity();
369 for_each(cbegin(bodies), cend(bodies), [&](
Body *b)
373 const auto underActiveTime = GetUnderActiveTime(*b, conf);
374 b->SetUnderActiveTime(underActiveTime);
375 minUnderActiveTime = std::min(minUnderActiveTime, underActiveTime);
378 return minUnderActiveTime;
384 for_each(cbegin(bodies), cend(bodies), [&](
Body *b)
399 void FlagContactsForFiltering(
const Body& bodyA,
const Body& bodyB) noexcept
404 const auto fA = contact->GetFixtureA();
405 const auto fB = contact->GetFixtureB();
406 const auto bA = fA->GetBody();
407 const auto bB = fB->GetBody();
408 const auto other = (bA != &bodyB)? bA: bB;
413 contact->FlagForFiltering();
422 m_minVertexRadius{def.minVertexRadius},
423 m_maxVertexRadius{def.maxVertexRadius}
425 if (def.minVertexRadius > def.maxVertexRadius)
427 throw InvalidArgument(
"max vertex radius must be >= min vertex radius");
429 m_proxyKeys.reserve(1024);
430 m_proxies.reserve(1024);
434 m_tree{other.m_tree},
435 m_destructionListener{other.m_destructionListener},
436 m_contactListener{other.m_contactListener},
437 m_flags{other.m_flags},
438 m_inv_dt0{other.m_inv_dt0},
439 m_minVertexRadius{other.m_minVertexRadius},
440 m_maxVertexRadius{other.m_maxVertexRadius}
442 auto bodyMap = std::map<const Body*, Body*>();
443 auto fixtureMap = std::map<const Fixture*, Fixture*>();
444 CopyBodies(bodyMap, fixtureMap, other.GetBodies());
445 CopyJoints(bodyMap, other.GetJoints());
446 CopyContacts(bodyMap, fixtureMap, other.GetContacts());
453 m_destructionListener = other.m_destructionListener;
454 m_contactListener = other.m_contactListener;
455 m_flags = other.m_flags;
456 m_inv_dt0 = other.m_inv_dt0;
457 m_minVertexRadius = other.m_minVertexRadius;
458 m_maxVertexRadius = other.m_maxVertexRadius;
459 m_tree = other.m_tree;
461 auto bodyMap = std::map<const Body*, Body*>();
462 auto fixtureMap = std::map<const Fixture*, Fixture*>();
463 CopyBodies(bodyMap, fixtureMap, other.
GetBodies());
465 CopyContacts(bodyMap, fixtureMap, other.
GetContacts());
479 throw WrongState(
"World::Clear: world is locked");
484 void World::InternalClear() noexcept
488 m_fixturesForProxies.clear();
489 m_bodiesForProxies.clear();
491 for_each(cbegin(m_joints), cend(m_joints), [&](
const Joint *j) {
492 if (m_destructionListener)
496 JointAtty::Destroy(j);
498 for_each(begin(m_bodies), end(m_bodies), [&](Bodies::value_type& body) {
500 BodyAtty::ClearContacts(b);
501 BodyAtty::ClearJoints(b);
502 BodyAtty::ClearFixtures(b, [&](Fixture& fixture) {
503 if (m_destructionListener)
507 DestroyProxies(fixture);
511 for_each(cbegin(m_bodies), cend(m_bodies), [&](
const Bodies::value_type& b) {
512 BodyAtty::Delete(
GetPtr(b));
514 for_each(cbegin(m_contacts), cend(m_contacts), [&](
const Contacts::value_type& c){
515 delete GetPtr(std::get<Contact*>(c));
523 void World::CopyBodies(std::map<const Body*, Body*>& bodyMap,
524 std::map<const Fixture*, Fixture*>& fixtureMap,
525 SizedRange<World::Bodies::const_iterator> range)
527 for (
const auto& otherBody: range)
530 for (
const auto& of:
GetRef(otherBody).GetFixtures())
532 const auto& otherFixture =
GetRef(of);
533 const auto shape = otherFixture.GetShape();
535 const auto newFixture = FixtureAtty::Create(*newBody, fixtureConf, shape);
536 BodyAtty::AddFixture(*newBody, newFixture);
537 fixtureMap[&otherFixture] = newFixture;
538 const auto childCount = otherFixture.GetProxyCount();
539 auto proxies = std::make_unique<FixtureProxy[]>(childCount);
540 for (
auto childIndex = decltype(childCount){0}; childIndex < childCount; ++childIndex)
542 const auto fp = otherFixture.GetProxy(childIndex);
543 proxies[childIndex] = FixtureProxy{fp.treeId};
544 const auto newData = DynamicTree::LeafData{newBody, newFixture, childIndex};
547 FixtureAtty::SetProxies(*newFixture, std::move(proxies), childCount);
550 bodyMap[
GetPtr(otherBody)] = newBody;
554 void World::CopyContacts(
const std::map<const Body*, Body*>& bodyMap,
555 const std::map<const Fixture*, Fixture*>& fixtureMap,
556 SizedRange<World::Contacts::const_iterator> range)
558 for (
const auto& contact: range)
560 auto& otherContact =
GetRef(std::get<Contact*>(contact));
561 const auto otherFixtureA = otherContact.GetFixtureA();
562 const auto otherFixtureB = otherContact.GetFixtureB();
563 const auto childIndexA = otherContact.GetChildIndexA();
564 const auto childIndexB = otherContact.GetChildIndexB();
565 const auto newFixtureA = fixtureMap.at(otherFixtureA);
566 const auto newFixtureB = fixtureMap.at(otherFixtureB);
567 const auto newBodyA = bodyMap.at(otherFixtureA->GetBody());
568 const auto newBodyB = bodyMap.at(otherFixtureB->GetBody());
569 const auto newContact =
new Contact{newFixtureA, childIndexA, newFixtureB, childIndexB};
573 const auto key = std::get<ContactKey>(contact);
576 BodyAtty::Insert(*newBodyA, key, newContact);
577 BodyAtty::Insert(*newBodyB, key, newContact);
580 newContact->SetFriction(otherContact.GetFriction());
581 newContact->SetRestitution(otherContact.GetRestitution());
582 newContact->SetTangentSpeed(otherContact.GetTangentSpeed());
583 auto& manifold = ContactAtty::GetMutableManifold(*newContact);
584 manifold = otherContact.GetManifold();
585 ContactAtty::CopyFlags(*newContact, otherContact);
586 if (otherContact.HasValidToi())
588 ContactAtty::SetToi(*newContact, otherContact.GetToi());
590 ContactAtty::SetToiCount(*newContact, otherContact.GetToiCount());
595 void World::CopyJoints(
const std::map<const Body*, Body*>& bodyMap,
596 SizedRange<World::Joints::const_iterator> range)
598 class JointCopier:
public ConstJointVisitor
602 JointCopier(
World& w, std::map<const Body*, Body*> bodies):
603 world{w}, bodyMap{std::move(bodies)}
608 void Visit(
const RevoluteJoint& oldJoint)
override
611 def.bodyA = bodyMap.at(def.bodyA);
612 def.bodyB = bodyMap.at(def.bodyB);
613 jointMap[&oldJoint] = Add(JointAtty::Create(def));
616 void Visit(
const PrismaticJoint& oldJoint)
override
619 def.bodyA = bodyMap.at(def.bodyA);
620 def.bodyB = bodyMap.at(def.bodyB);
621 jointMap[&oldJoint] = Add(JointAtty::Create(def));
624 void Visit(
const DistanceJoint& oldJoint)
override
627 def.bodyA = bodyMap.at(def.bodyA);
628 def.bodyB = bodyMap.at(def.bodyB);
629 jointMap[&oldJoint] = Add(JointAtty::Create(def));
632 void Visit(
const PulleyJoint& oldJoint)
override
635 def.bodyA = bodyMap.at(def.bodyA);
636 def.bodyB = bodyMap.at(def.bodyB);
637 jointMap[&oldJoint] = Add(JointAtty::Create(def));
640 void Visit(
const TargetJoint& oldJoint)
override
643 def.bodyA = (def.bodyA)? bodyMap.at(def.bodyA):
nullptr;
644 def.bodyB = (def.bodyB)? bodyMap.at(def.bodyB):
nullptr;
645 jointMap[&oldJoint] = Add(JointAtty::Create(def));
648 void Visit(
const GearJoint& oldJoint)
override
651 def.bodyA = bodyMap.at(def.bodyA);
652 def.bodyB = bodyMap.at(def.bodyB);
653 def.joint1 = jointMap.at(def.joint1);
654 def.joint2 = jointMap.at(def.joint2);
655 jointMap[&oldJoint] = Add(JointAtty::Create(def));
658 void Visit(
const WheelJoint& oldJoint)
override
661 def.bodyA = bodyMap.at(def.bodyA);
662 def.bodyB = bodyMap.at(def.bodyB);
663 jointMap[&oldJoint] = Add(JointAtty::Create(def));
666 void Visit(
const WeldJoint& oldJoint)
override
669 def.bodyA = bodyMap.at(def.bodyA);
670 def.bodyB = bodyMap.at(def.bodyB);
671 jointMap[&oldJoint] = Add(JointAtty::Create(def));
674 void Visit(
const FrictionJoint& oldJoint)
override
677 def.bodyA = bodyMap.at(def.bodyA);
678 def.bodyB = bodyMap.at(def.bodyB);
679 jointMap[&oldJoint] = Add(JointAtty::Create(def));
682 void Visit(
const RopeJoint& oldJoint)
override
685 def.bodyA = bodyMap.at(def.bodyA);
686 def.bodyB = bodyMap.at(def.bodyB);
687 jointMap[&oldJoint] = Add(JointAtty::Create(def));
690 void Visit(
const MotorJoint& oldJoint)
override
693 def.bodyA = bodyMap.at(def.bodyA);
694 def.bodyB = bodyMap.at(def.bodyB);
695 jointMap[&oldJoint] = Add(JointAtty::Create(def));
698 Joint* Add(Joint* newJoint)
705 const std::map<const Body*, Body*> bodyMap;
706 std::map<const Joint*, Joint*> jointMap;
709 auto copier = JointCopier{*
this, bodyMap};
710 auto jointMap = std::map<const Joint*, Joint*>();
711 for (
const auto& otherJoint: range)
713 otherJoint->Accept(copier);
721 throw WrongState(
"World::CreateBody: world is locked");
726 throw LengthError(
"World::CreateBody: operation would exceed MaxBodies");
729 auto& b = *BodyAtty::CreateBody(
this, def);
739 m_bodies.push_back(&b);
744 void World::Remove(
const Body& b) noexcept
746 UnregisterForProxies(b);
747 const auto it = find_if(cbegin(m_bodies), cend(m_bodies), [&](
const Bodies::value_type& body) {
748 return GetPtr(body) == &b;
750 if (it != end(m_bodies))
752 BodyAtty::Delete(
GetPtr(*it));
764 throw WrongState(
"World::Destroy: world is locked");
768 BodyAtty::ClearJoints(*body, [&](
Joint& joint) {
769 if (m_destructionListener)
773 InternalDestroy(joint);
777 BodyAtty::EraseContacts(*body, [&](
Contact& contact) {
783 BodyAtty::ClearFixtures(*body, [&](
Fixture& fixture) {
784 if (m_destructionListener)
788 UnregisterForProxies(fixture);
789 DestroyProxies(fixture);
790 FixtureAtty::Delete(&fixture);
800 throw WrongState(
"World::CreateJoint: world is locked");
805 throw LengthError(
"World::CreateJoint: operation would exceed MaxJoints");
809 const auto j = JointAtty::Create(def);
819 FlagContactsForFiltering(*bodyA, *bodyB);
825 bool World::Add(
Joint* j)
828 m_joints.push_back(j);
831 BodyAtty::Insert(bodyA, j);
832 BodyAtty::Insert(bodyB, j);
836 void World::Remove(
const Joint& j) noexcept
838 const auto endIter = cend(m_joints);
839 const auto iter = find(cbegin(m_joints), endIter, &j);
840 assert(iter != endIter);
841 m_joints.erase(iter);
850 throw WrongState(
"World::Destroy: world is locked");
852 InternalDestroy(*joint);
856 void World::InternalDestroy(
Joint& joint) noexcept
861 const auto bodyA = joint.GetBodyA();
862 const auto bodyB = joint.GetBodyB();
868 BodyAtty::Erase(*bodyA, &joint);
873 BodyAtty::Erase(*bodyB, &joint);
876 const auto collideConnected = joint.GetCollideConnected();
878 JointAtty::Destroy(&joint);
881 if ((!collideConnected) && bodyA && bodyB)
883 FlagContactsForFiltering(*bodyA, *bodyB);
887 void World::AddToIsland(Island& island, Body& seed,
888 Bodies::size_type& remNumBodies,
889 Contacts::size_type& remNumContacts,
890 Joints::size_type& remNumJoints)
892 assert(!IsIslanded(&seed));
893 assert(seed.IsSpeedable());
894 assert(seed.IsAwake());
895 assert(seed.IsEnabled());
896 assert(remNumBodies != 0);
902 auto stack = BodyStack{};
903 stack.reserve(remNumBodies);
905 stack.push_back(&seed);
907 AddToIsland(island, stack, remNumBodies, remNumContacts, remNumJoints);
910 void World::AddToIsland(Island& island, BodyStack& stack,
911 Bodies::size_type& remNumBodies,
912 Contacts::size_type& remNumContacts,
913 Joints::size_type& remNumJoints)
915 while (!
empty(stack))
918 const auto b = stack.back();
922 assert(b->IsEnabled());
923 island.m_bodies.push_back(b);
924 assert(remNumBodies > 0);
929 if (!b->IsSpeedable())
935 BodyAtty::SetAwakeFlag(*b);
937 const auto oldNumContacts =
size(island.m_contacts);
939 AddContactsToIsland(island, stack, b);
941 const auto newNumContacts =
size(island.m_contacts);
942 assert(newNumContacts >= oldNumContacts);
943 const auto netNumContacts = newNumContacts - oldNumContacts;
944 assert(remNumContacts >= netNumContacts);
945 remNumContacts -= netNumContacts;
947 const auto numJoints =
size(island.m_joints);
949 AddJointsToIsland(island, stack, b);
951 remNumJoints -=
size(island.m_joints) - numJoints;
955 void World::AddContactsToIsland(Island& island, BodyStack& stack,
const Body* b)
957 const auto contacts = b->GetContacts();
958 for_each(cbegin(contacts), cend(contacts), [&](
const KeyedContactPtr& ci) {
960 if (!IsIslanded(contact) && contact->IsEnabled() && contact->IsTouching())
962 const auto fA = contact->GetFixtureA();
963 const auto fB = contact->GetFixtureB();
964 if (!fA->IsSensor() && !fB->IsSensor())
966 const auto bA = fA->GetBody();
967 const auto bB = fB->GetBody();
968 const auto other = (bA != b)? bA: bB;
969 island.m_contacts.push_back(contact);
970 SetIslanded(contact);
971 if (!IsIslanded(other))
973 stack.push_back(other);
981 void World::AddJointsToIsland(Island& island, BodyStack& stack,
const Body* b)
983 const auto joints = b->GetJoints();
986 const auto other = std::get<Body*>(ji);
987 const auto joint = std::get<Joint*>(ji);
988 assert(!other || other->IsEnabled() || !other->IsAwake());
989 if (!IsIslanded(joint) && (!other || other->IsEnabled()))
991 island.m_joints.push_back(joint);
993 if (other && !IsIslanded(other))
996 const auto bodyA = joint->GetBodyA();
997 const auto bodyB = joint->GetBodyB();
998 const auto rwOther = bodyA != b? bodyA: bodyB;
999 assert(rwOther == other);
1000 stack.push_back(rwOther);
1001 SetIslanded(rwOther);
1007 World::Bodies::size_type World::RemoveUnspeedablesFromIslanded(
const std::vector<Body*>& bodies)
1009 auto numRemoved = Bodies::size_type{0};
1010 for_each(begin(bodies), end(bodies), [&](Body* body) {
1011 if (!body->IsSpeedable())
1014 UnsetIslanded(body);
1021 RegStepStats World::SolveReg(
const StepConf& conf)
1023 auto stats = RegStepStats{};
1024 assert(stats.islandsFound == 0);
1025 assert(stats.islandsSolved == 0);
1027 auto remNumBodies =
size(m_bodies);
1028 auto remNumContacts =
size(m_contacts);
1029 auto remNumJoints =
size(m_joints);
1035 for_each(begin(m_bodies), end(m_bodies), [](Bodies::value_type& b) {
1036 BodyAtty::UnsetIslanded(
GetRef(b));
1038 for_each(begin(m_contacts), end(m_contacts), [](Contacts::value_type& c) {
1039 ContactAtty::UnsetIslanded(
GetRef(std::get<Contact*>(c)));
1041 for_each(begin(m_joints), end(m_joints), [](Joints::value_type& j) {
1042 JointAtty::UnsetIslanded(
GetRef(j));
1045 #if defined(DO_THREADED)
1046 std::vector<std::future<World::IslandSolverResults>> futures;
1047 futures.reserve(remNumBodies);
1050 for (
auto&& b: m_bodies)
1053 assert(!body.IsAwake() || body.IsSpeedable());
1054 if (!IsIslanded(&body) && body.IsAwake() && body.IsEnabled())
1056 ++stats.islandsFound;
1059 Island island(remNumBodies, remNumContacts, remNumJoints);
1061 AddToIsland(island, body, remNumBodies, remNumContacts, remNumJoints);
1062 remNumBodies += RemoveUnspeedablesFromIslanded(island.m_bodies);
1064 #if defined(DO_THREADED)
1066 futures.push_back(std::async(std::launch::async, &World::SolveRegIslandViaGS,
1067 this, conf, island));
1069 const auto solverResults = SolveRegIslandViaGS(conf, island);
1070 Update(stats, solverResults);
1075 #if defined(DO_THREADED)
1076 for (
auto&& future: futures)
1078 const auto solverResults = future.get();
1079 Update(stats, solverResults);
1083 for (
auto&& b: m_bodies)
1087 if (IsIslanded(&body) && body.IsSpeedable())
1090 stats.proxiesMoved += Synchronize(body,
GetTransform0(body.GetSweep()), body.GetTransformation(),
1091 conf.displaceMultiplier, conf.aabbExtension);
1096 stats.contactsAdded = FindNewContacts();
1101 IslandStats World::SolveRegIslandViaGS(
const StepConf& conf, Island island)
1103 assert(!
empty(island.m_bodies) || !
empty(island.m_contacts) || !
empty(island.m_joints));
1105 auto results = IslandStats{};
1106 results.positionIterations = conf.regPositionIterations;
1107 const auto h = conf.GetTime();
1110 for_each(cbegin(island.m_bodies), cend(island.m_bodies), [&](Body* body) {
1111 BodyAtty::SetPosition0(*body, GetPosition1(*body));
1115 auto bodyConstraints = GetBodyConstraints(island.m_bodies, h,
GetMovementConf(conf));
1116 auto bodyConstraintsMap = GetBodyConstraintsMap(island.m_bodies, bodyConstraints);
1117 auto posConstraints = GetPositionConstraints(island.m_contacts, bodyConstraintsMap);
1118 auto velConstraints = GetVelocityConstraints(island.m_contacts, bodyConstraintsMap,
1121 if (conf.doWarmStart)
1123 WarmStartVelocities(velConstraints);
1128 for_each(cbegin(island.m_joints), cend(island.m_joints), [&](Joint* joint) {
1129 JointAtty::InitVelocityConstraints(*joint, bodyConstraintsMap, conf, psConf);
1132 results.velocityIterations = conf.regVelocityIterations;
1133 for (
auto i = decltype(conf.regVelocityIterations){0}; i < conf.regVelocityIterations; ++i)
1135 auto jointsOkay =
true;
1136 for_each(cbegin(island.m_joints), cend(island.m_joints), [&](Joint* j) {
1137 jointsOkay &= JointAtty::SolveVelocityConstraints(*j, bodyConstraintsMap, conf);
1142 const auto newIncImpulse = SolveVelocityConstraintsViaGS(velConstraints);
1143 results.maxIncImpulse = std::max(results.maxIncImpulse, newIncImpulse);
1145 if (jointsOkay && (newIncImpulse <= conf.regMinMomentum))
1152 results.velocityIterations = i + 1;
1158 IntegratePositions(bodyConstraints, h);
1161 for (
auto i = decltype(conf.regPositionIterations){0}; i < conf.regPositionIterations; ++i)
1163 const auto minSeparation = SolvePositionConstraintsViaGS(posConstraints, psConf);
1164 results.minSeparation = std::min(results.minSeparation, minSeparation);
1165 const auto contactsOkay = (minSeparation >= conf.regMinSeparation);
1167 auto jointsOkay =
true;
1168 for_each(cbegin(island.m_joints), cend(island.m_joints), [&](Joint* j) {
1169 jointsOkay &= JointAtty::SolvePositionConstraints(*j, bodyConstraintsMap, psConf);
1172 if (contactsOkay && jointsOkay)
1175 results.positionIterations = i + 1;
1176 results.solved =
true;
1182 for_each(cbegin(velConstraints), cend(velConstraints), [&](
const VelocityConstraint& vc) {
1183 const auto i =
static_cast<VelocityConstraints::size_type
>(&vc - data(velConstraints));
1184 auto& manifold = ContactAtty::GetMutableManifold(*island.m_contacts[i]);
1185 AssignImpulses(manifold, vc);
1188 for_each(cbegin(bodyConstraints), cend(bodyConstraints), [&](
const BodyConstraint& bc) {
1189 const auto i =
static_cast<size_t>(&bc - data(bodyConstraints));
1190 assert(i <
size(bodyConstraints));
1193 UpdateBody(*island.m_bodies[i], bc.GetPosition(), bc.GetVelocity());
1198 if (m_contactListener)
1200 Report(*m_contactListener, island.m_contacts, velConstraints,
1205 const auto minUnderActiveTime = UpdateUnderActiveTimes(island.m_bodies, conf);
1206 if ((minUnderActiveTime >= conf.minStillTimeToSleep) && results.solved)
1208 results.bodiesSlept =
static_cast<decltype(results.bodiesSlept)
>(Sleepem(island.m_bodies));
1214 void World::ResetBodiesForSolveTOI() noexcept
1216 for_each(begin(m_bodies), end(m_bodies), [&](Bodies::value_type& body) {
1218 BodyAtty::UnsetIslanded(b);
1219 BodyAtty::ResetAlpha0(b);
1223 void World::ResetContactsForSolveTOI() noexcept
1225 for_each(begin(m_contacts), end(m_contacts), [&](Contacts::value_type &c) {
1226 auto& contact =
GetRef(std::get<Contact*>(c));
1227 ContactAtty::UnsetIslanded(contact);
1228 ContactAtty::UnsetToi(contact);
1229 ContactAtty::ResetToiCount(contact);
1233 World::UpdateContactsData World::UpdateContactTOIs(
const StepConf& conf)
1235 auto results = UpdateContactsData{};
1239 for (
auto&& contact: m_contacts)
1241 auto& c =
GetRef(std::get<Contact*>(contact));
1242 if (c.HasValidToi())
1244 ++results.numValidTOI;
1251 if (c.GetToiCount() >= conf.maxSubSteps)
1257 ++results.numAtMaxSubSteps;
1261 const auto fA = c.GetFixtureA();
1262 const auto fB = c.GetFixtureB();
1263 const auto bA = fA->GetBody();
1264 const auto bB = fB->GetBody();
1272 const auto alpha0 = std::max(bA->GetSweep().GetAlpha0(), bB->GetSweep().GetAlpha0());
1273 assert(alpha0 >= 0 && alpha0 < 1);
1274 BodyAtty::Advance0(*bA, alpha0);
1275 BodyAtty::Advance0(*bB, alpha0);
1279 const auto output =
CalcToi(c, toiConf);
1283 const auto toi = IsValidForTime(output.state)?
1284 std::min(alpha0 + (1 - alpha0) * output.time,
Real{1}):
Real{1};
1285 assert(toi >= alpha0 && toi <= 1);
1286 ContactAtty::SetToi(c, toi);
1288 results.maxDistIters = std::max(results.maxDistIters, output.stats.max_dist_iters);
1289 results.maxToiIters = std::max(results.maxToiIters, output.stats.toi_iters);
1290 results.maxRootIters = std::max(results.maxRootIters, output.stats.max_root_iters);
1291 ++results.numUpdatedTOI;
1297 World::ContactToiData World::GetSoonestContact() const noexcept
1300 auto found =
static_cast<Contact*
>(
nullptr);
1302 for (
auto&& contact: m_contacts)
1304 const auto c =
GetPtr(std::get<Contact*>(contact));
1305 if (c->HasValidToi())
1307 const auto toi = c->GetToi();
1314 else if (minToi == toi)
1321 return ContactToiData{found, minToi, count};
1324 ToiStepStats World::SolveToi(
const StepConf& conf)
1326 auto stats = ToiStepStats{};
1328 if (IsStepComplete())
1330 ResetBodiesForSolveTOI();
1331 ResetContactsForSolveTOI();
1334 const auto subStepping = GetSubStepping();
1339 const auto updateData = UpdateContactTOIs(conf);
1340 stats.contactsAtMaxSubSteps += updateData.numAtMaxSubSteps;
1341 stats.contactsUpdatedToi += updateData.numUpdatedTOI;
1342 stats.maxDistIters = std::max(stats.maxDistIters, updateData.maxDistIters);
1343 stats.maxRootIters = std::max(stats.maxRootIters, updateData.maxRootIters);
1344 stats.maxToiIters = std::max(stats.maxToiIters, updateData.maxToiIters);
1346 const auto next = GetSoonestContact();
1347 const auto contact = next.contact;
1348 const auto ncount = next.simultaneous;
1352 SetStepComplete(
true);
1356 stats.maxSimulContacts = std::max(stats.maxSimulContacts,
1357 static_cast<decltype(stats.maxSimulContacts)
>(ncount));
1358 stats.contactsFound += ncount;
1359 auto islandsFound = 0u;
1360 if (!IsIslanded(contact))
1366 assert(contact->IsEnabled());
1371 const auto solverResults = SolveToi(conf, *contact);
1372 stats.minSeparation = std::min(stats.minSeparation, solverResults.minSeparation);
1373 stats.maxIncImpulse = std::max(stats.maxIncImpulse, solverResults.maxIncImpulse);
1374 stats.islandsSolved += solverResults.solved;
1375 stats.sumPosIters += solverResults.positionIterations;
1376 stats.sumVelIters += solverResults.velocityIterations;
1377 if ((solverResults.positionIterations > 0) || (solverResults.velocityIterations > 0))
1381 stats.contactsUpdatedTouching += solverResults.contactsUpdated;
1382 stats.contactsSkippedTouching += solverResults.contactsSkipped;
1384 stats.islandsFound += islandsFound;
1387 for (
auto&& b: m_bodies)
1390 if (IsIslanded(&body))
1392 UnsetIslanded(&body);
1393 if (body.IsAccelerable())
1396 const auto xfm1 = body.GetTransformation();
1397 stats.proxiesMoved += Synchronize(body, xfm0, xfm1,
1398 conf.displaceMultiplier, conf.aabbExtension);
1399 ResetContactsForSolveTOI(body);
1406 stats.contactsAdded += FindNewContacts();
1410 SetStepComplete(
false);
1417 IslandStats World::SolveToi(
const StepConf& conf, Contact& contact)
1433 assert(contact.IsEnabled());
1437 assert(!IsIslanded(&contact));
1439 const auto toi = contact.GetToi();
1440 const auto bA = contact.GetFixtureA()->GetBody();
1441 const auto bB = contact.GetFixtureB()->GetBody();
1447 const auto backupA = bA->GetSweep();
1448 const auto backupB = bB->GetSweep();
1451 assert(toi != 0 || (bA->GetSweep().GetAlpha0() == 0 && bB->GetSweep().GetAlpha0() == 0));
1452 BodyAtty::Advance(*bA, toi);
1453 BodyAtty::Advance(*bB, toi);
1456 contact.SetEnabled();
1457 if (contact.NeedsUpdating())
1466 ContactAtty::UnsetToi(contact);
1467 ContactAtty::IncrementToiCount(contact);
1478 if (!contact.IsEnabled() || !contact.IsTouching())
1481 contact.UnsetEnabled();
1482 BodyAtty::Restore(*bA, backupA);
1483 BodyAtty::Restore(*bB, backupB);
1484 auto results = IslandStats{};
1485 results.contactsUpdated += contactsUpdated;
1486 results.contactsSkipped += contactsSkipped;
1491 else if (!contact.IsTouching())
1493 const auto newManifold = contact.Evaluate();
1494 assert(contact.IsTouching());
1495 return IslandSolverResults{};
1499 if (bA->IsSpeedable())
1501 BodyAtty::SetAwakeFlag(*bA);
1507 if (bB->IsSpeedable())
1509 BodyAtty::SetAwakeFlag(*bB);
1516 Island island(
size(m_bodies),
size(m_contacts), 0);
1519 assert(!IsIslanded(bA));
1520 assert(!IsIslanded(bB));
1522 island.m_bodies.push_back(bA);
1524 island.m_bodies.push_back(bB);
1526 island.m_contacts.push_back(&contact);
1527 SetIslanded(&contact);
1532 if (bA->IsAccelerable())
1534 const auto procOut = ProcessContactsForTOI(island, *bA, toi, conf);
1535 contactsUpdated += procOut.contactsUpdated;
1536 contactsSkipped += procOut.contactsSkipped;
1538 if (bB->IsAccelerable())
1540 const auto procOut = ProcessContactsForTOI(island, *bB, toi, conf);
1541 contactsUpdated += procOut.contactsUpdated;
1542 contactsSkipped += procOut.contactsSkipped;
1545 RemoveUnspeedablesFromIslanded(island.m_bodies);
1553 auto subConf = StepConf{conf};
1554 auto results = SolveToiViaGS(subConf.SetTime((1 - toi) * conf.GetTime()), island);
1555 results.contactsUpdated += contactsUpdated;
1556 results.contactsSkipped += contactsSkipped;
1560 void World::UpdateBody(Body& body,
const Position& pos,
const Velocity& vel)
1564 BodyAtty::SetVelocity(body, vel);
1565 BodyAtty::SetPosition1(body, pos);
1569 IslandStats World::SolveToiViaGS(
const StepConf& conf, Island& island)
1571 auto results = IslandStats{};
1579 auto bodyConstraints = GetBodyConstraints(island.m_bodies, 0_s,
GetMovementConf(conf));
1580 auto bodyConstraintsMap = GetBodyConstraintsMap(island.m_bodies, bodyConstraints);
1584 for (
auto&& contact: island.m_contacts)
1586 const auto fixtureA = contact->GetFixtureA();
1587 const auto fixtureB = contact->GetFixtureB();
1588 const auto bodyA = fixtureA->GetBody();
1589 const auto bodyB = fixtureB->GetBody();
1596 auto posConstraints = GetPositionConstraints(island.m_contacts, bodyConstraintsMap);
1599 assert(results.minSeparation == std::numeric_limits<Length>::infinity());
1600 assert(results.solved ==
false);
1601 results.positionIterations = conf.toiPositionIterations;
1605 for (
auto i = decltype(conf.toiPositionIterations){0}; i < conf.toiPositionIterations; ++i)
1617 const auto minSeparation = SolvePositionConstraintsViaGS(posConstraints, psConf);
1618 results.minSeparation = std::min(results.minSeparation, minSeparation);
1619 if (minSeparation >= conf.toiMinSeparation)
1622 results.positionIterations = i + 1;
1623 results.solved =
true;
1634 for (
auto&& contact: island.m_contacts)
1636 const auto fixtureA = contact->GetFixtureA();
1637 const auto fixtureB = contact->GetFixtureB();
1638 const auto bodyA = fixtureA->GetBody();
1639 const auto bodyB = fixtureB->GetBody();
1641 BodyAtty::SetPosition0(*bodyA, bodyConstraintsMap.at(bodyA).GetPosition());
1642 BodyAtty::SetPosition0(*bodyB, bodyConstraintsMap.at(bodyB).GetPosition());
1645 for_each(cbegin(bodyConstraints), cend(bodyConstraints), [&](
const BodyConstraint& bc) {
1646 const auto i =
static_cast<size_t>(&bc - data(bodyConstraints));
1647 assert(i <
size(bodyConstraints));
1648 BodyAtty::SetPosition0(*island.m_bodies[i], bc.GetPosition());
1652 auto velConstraints = GetVelocityConstraints(island.m_contacts, bodyConstraintsMap,
1659 assert(results.maxIncImpulse == 0_Ns);
1660 results.velocityIterations = conf.toiVelocityIterations;
1661 for (
auto i = decltype(conf.toiVelocityIterations){0}; i < conf.toiVelocityIterations; ++i)
1663 const auto newIncImpulse = SolveVelocityConstraintsViaGS(velConstraints);
1664 if (newIncImpulse <= conf.toiMinMomentum)
1670 results.velocityIterations = i + 1;
1673 results.maxIncImpulse = std::max(results.maxIncImpulse, newIncImpulse);
1678 IntegratePositions(bodyConstraints, conf.GetTime());
1680 for_each(cbegin(bodyConstraints), cend(bodyConstraints), [&](
const BodyConstraint& bc) {
1681 const auto i =
static_cast<size_t>(&bc - data(bodyConstraints));
1682 assert(i <
size(bodyConstraints));
1683 UpdateBody(*island.m_bodies[i], bc.GetPosition(), bc.GetVelocity());
1686 if (m_contactListener)
1688 Report(*m_contactListener, island.m_contacts, velConstraints, results.positionIterations);
1694 void World::ResetContactsForSolveTOI(Body& body) noexcept
1697 const auto contacts = body.GetContacts();
1700 UnsetIslanded(contact);
1701 ContactAtty::UnsetToi(*contact);
1705 World::ProcessContactsOutput
1706 World::ProcessContactsForTOI(Island& island, Body& body,
Real toi,
1707 const StepConf& conf)
1709 assert(IsIslanded(&body));
1710 assert(body.IsAccelerable());
1711 assert(toi >= 0 && toi <= 1);
1713 auto results = ProcessContactsOutput{};
1714 assert(results.contactsUpdated == 0);
1715 assert(results.contactsSkipped == 0);
1717 const auto updateConf = Contact::GetUpdateConf(conf);
1719 auto processContactFunc = [&](Contact* contact, Body* other)
1721 const auto otherIslanded = IsIslanded(other);
1723 const auto backup = other->GetSweep();
1724 if (!otherIslanded )
1726 BodyAtty::Advance(*other, toi);
1730 contact->SetEnabled();
1731 if (contact->NeedsUpdating())
1734 ++results.contactsUpdated;
1738 ++results.contactsSkipped;
1742 if (!contact->IsEnabled() || !contact->IsTouching())
1744 BodyAtty::Restore(*other, backup);
1748 island.m_contacts.push_back(contact);
1749 SetIslanded(contact);
1752 if (other->IsSpeedable())
1754 BodyAtty::SetAwakeFlag(*other);
1756 island.m_bodies.push_back(other);
1759 if (other->IsAccelerable())
1761 contactsUpdated += ProcessContactsForTOI(island, *other, toi);
1775 assert(
Count(island, other) > 0);
1781 const auto bodyImpenetrable = body.IsImpenetrable();
1782 for (
auto&& ci: body.GetContacts())
1785 if (!IsIslanded(contact))
1787 const auto fA = contact->GetFixtureA();
1788 const auto fB = contact->GetFixtureB();
1789 if (!fA->IsSensor() && !fB->IsSensor())
1791 const auto bA = fA->GetBody();
1792 const auto bB = fB->GetBody();
1793 const auto other = (bA != &body)? bA: bB;
1794 if (bodyImpenetrable || other->IsImpenetrable())
1796 processContactFunc(contact, other);
1806 assert((
Length{m_maxVertexRadius} *
Real{2}) +
1811 throw WrongState(
"World::Step: world is locked");
1817 FlagGuard<decltype(m_flags)> flagGaurd(m_flags, e_locked);
1819 CreateAndDestroyProxies(conf);
1820 stepStats.pre.proxiesMoved = SynchronizeProxies(conf);
1825 const auto destroyStats = DestroyContacts(m_contacts);
1826 stepStats.pre.destroyed = destroyStats.erased;
1829 if (HasNewFixtures())
1835 stepStats.pre.added = FindNewContacts();
1843 const auto updateStats = UpdateContacts(m_contacts, conf);
1844 stepStats.pre.ignored = updateStats.ignored;
1845 stepStats.pre.updated = updateStats.updated;
1846 stepStats.pre.skipped = updateStats.skipped;
1849 if (IsStepComplete())
1851 stepStats.reg = SolveReg(conf);
1857 stepStats.toi = SolveToi(conf);
1868 throw WrongState(
"World::ShiftOrigin: world is locked");
1871 const auto bodies = GetBodies();
1872 for (
auto&& body: bodies)
1876 auto transformation = b.GetTransformation();
1877 transformation.p -= newOrigin;
1880 auto sweep = b.GetSweep();
1881 sweep.pos0.linear -= newOrigin;
1882 sweep.pos1.linear -= newOrigin;
1883 BodyAtty::SetSweep(b, sweep);
1886 for_each(begin(m_joints), end(m_joints), [&](Joints::value_type& j) {
1887 GetRef(j).ShiftOrigin(newOrigin);
1890 m_tree.ShiftOrigin(newOrigin);
1893 void World::InternalDestroy(
Contact* contact,
Body* from)
1895 if (m_contactListener && contact->
IsTouching())
1898 m_contactListener->EndContact(*contact);
1903 const auto bodyA = fixtureA->
GetBody();
1904 const auto bodyB = fixtureB->GetBody();
1908 BodyAtty::Erase(*bodyA, contact);
1912 BodyAtty::Erase(*bodyB, contact);
1916 !fixtureA->IsSensor() && !fixtureB->IsSensor())
1927 void World::Destroy(Contact* contact, Body* from)
1931 InternalDestroy(contact, from);
1933 const auto it = find_if(cbegin(m_contacts), cend(m_contacts),
1934 [&](
const Contacts::value_type& c) {
1935 return GetPtr(std::get<Contact*>(c)) == contact;
1937 if (it != cend(m_contacts))
1939 m_contacts.erase(it);
1943 World::DestroyContactsStats World::DestroyContacts(Contacts& contacts)
1945 const auto beforeSize =
size(contacts);
1946 contacts.erase(std::remove_if(begin(contacts), end(contacts), [&](Contacts::value_type& c)
1948 const auto key = std::get<ContactKey>(c);
1949 auto& contact =
GetRef(std::get<Contact*>(c));
1951 if (!
TestOverlap(m_tree, key.GetMin(), key.GetMax()))
1954 InternalDestroy(&contact);
1959 if (contact.NeedsFiltering())
1961 const auto fixtureA = contact.GetFixtureA();
1962 const auto fixtureB = contact.GetFixtureB();
1963 const auto bodyA = fixtureA->GetBody();
1964 const auto bodyB = fixtureB->GetBody();
1968 InternalDestroy(&contact);
1971 ContactAtty::UnflagForFiltering(contact);
1976 const auto afterSize =
size(contacts);
1978 auto stats = DestroyContactsStats{};
1980 stats.erased =
static_cast<ContactCounter>(beforeSize - afterSize);
1984 World::UpdateContactsStats World::UpdateContacts(Contacts& contacts,
const StepConf& conf)
1987 atomic<uint32_t> ignored;
1988 atomic<uint32_t> updated;
1989 atomic<uint32_t> skipped;
1991 auto ignored = uint32_t{0};
1992 auto updated = uint32_t{0};
1993 auto skipped = uint32_t{0};
1996 const auto updateConf = Contact::GetUpdateConf(conf);
1998 #if defined(DO_THREADED)
1999 std::vector<Contact*> contactsNeedingUpdate;
2000 contactsNeedingUpdate.reserve(
size(contacts));
2001 std::vector<std::future<void>> futures;
2002 futures.reserve(
size(contacts));
2006 for_each( begin(contacts), end(contacts),
2007 [&](Contacts::value_type& c) {
2008 auto& contact =
GetRef(std::get<Contact*>(c));
2013 const auto bodyA =
GetBodyA(contact);
2014 const auto bodyB =
GetBodyB(contact);
2018 assert(!bodyA->IsAwake() || bodyA->IsSpeedable());
2019 assert(!bodyB->IsAwake() || bodyB->IsSpeedable());
2020 if (!bodyA->IsAwake() && !bodyB->IsAwake())
2022 assert(!contact.HasValidToi());
2031 contact.SetEnabled();
2039 if (contact.NeedsUpdating())
2042 #if defined(DO_THREADED)
2043 contactsNeedingUpdate.push_back(&contact);
2058 #if defined(DO_THREADED)
2059 auto numJobs =
size(contactsNeedingUpdate);
2060 const auto jobsPerCore = numJobs / 4;
2061 for (
auto i = decltype(numJobs){0}; numJobs > 0 && i < 3; ++i)
2063 futures.push_back(std::async(std::launch::async, [=]{
2064 const auto offset = jobsPerCore * i;
2065 for (
auto j = decltype(jobsPerCore){0}; j < jobsPerCore; ++j)
2070 numJobs -= jobsPerCore;
2074 futures.push_back(std::async(std::launch::async, [=]{
2075 const auto offset = jobsPerCore * 3;
2076 for (
auto j = decltype(numJobs){0}; j < numJobs; ++j)
2082 for (
auto&& future: futures)
2088 return UpdateContactsStats{
2095 void World::UnregisterForProcessing(ProxyId pid) noexcept
2097 const auto itEnd = end(m_proxies);
2098 auto it = find( begin(m_proxies), itEnd, pid);
2101 *it = DynamicTree::GetInvalidSize();
2107 m_proxyKeys.clear();
2113 for_each(cbegin(m_proxies), cend(m_proxies), [&](ProxyId pid) {
2114 const auto body0 = m_tree.GetLeafData(pid).body;
2115 const auto aabb = m_tree.GetAABB(pid);
2116 Query(m_tree, aabb, [&](DynamicTree::Size nodeId) {
2117 const auto body1 = m_tree.GetLeafData(nodeId).body;
2119 if ((nodeId != pid) && (body0 != body1))
2121 m_proxyKeys.push_back(ContactKey{nodeId, pid});
2123 return DynamicTreeOpcode::Continue;
2129 sort(begin(m_proxyKeys), end(m_proxyKeys));
2130 m_proxyKeys.erase(unique(begin(m_proxyKeys), end(m_proxyKeys)), end(m_proxyKeys));
2132 const auto numContactsBefore =
size(m_contacts);
2133 for_each(cbegin(m_proxyKeys), cend(m_proxyKeys), [&](ContactKey key)
2137 const auto numContactsAfter =
size(m_contacts);
2138 return static_cast<ContactCounter>(numContactsAfter - numContactsBefore);
2141 bool World::Add(ContactKey key)
2143 const auto minKeyLeafData = m_tree.GetLeafData(key.GetMin());
2144 const auto maxKeyLeafData = m_tree.GetLeafData(key.GetMax());
2146 const auto fixtureA = minKeyLeafData.fixture;
2147 const auto indexA = minKeyLeafData.childIndex;
2148 const auto fixtureB = maxKeyLeafData.fixture;
2149 const auto indexB = maxKeyLeafData.childIndex;
2151 const auto bodyA = minKeyLeafData.body;
2152 const auto bodyB = maxKeyLeafData.body;
2162 assert(bodyA != bodyB);
2204 const auto searchBody = (
size(bodyA->GetContacts()) <
size(bodyB->GetContacts()))?
2207 const auto contacts = searchBody->GetContacts();
2208 const auto it = find_if(cbegin(contacts), cend(contacts), [&](
KeyedContactPtr ci) {
2209 return std::get<ContactKey>(ci) == key;
2211 if (it != cend(contacts))
2223 const auto contact =
new Contact{fixtureA, indexA, fixtureB, indexB};
2234 BodyAtty::Insert(*bodyA, key, contact);
2235 BodyAtty::Insert(*bodyB, key, contact);
2238 if (!fixtureA->IsSensor() && !fixtureB->IsSensor())
2240 if (bodyA->IsSpeedable())
2242 BodyAtty::SetAwakeFlag(*bodyA);
2244 if (bodyB->IsSpeedable())
2246 BodyAtty::SetAwakeFlag(*bodyB);
2254 void World::RegisterForProxies(Fixture& fixture)
2256 assert(fixture.GetBody()->GetWorld() ==
this);
2257 m_fixturesForProxies.push_back(&fixture);
2260 void World::UnregisterForProxies(
const Fixture& fixture)
2262 assert(fixture.GetBody()->GetWorld() ==
this);
2263 const auto first = remove(begin(m_fixturesForProxies), end(m_fixturesForProxies), &fixture);
2264 m_fixturesForProxies.erase(first, end(m_fixturesForProxies));
2267 void World::RegisterForProxies(Body& body)
2269 assert(body.GetWorld() ==
this);
2270 m_bodiesForProxies.push_back(&body);
2273 void World::UnregisterForProxies(
const Body& body)
2275 assert(body.GetWorld() ==
this);
2276 const auto first = remove(begin(m_bodiesForProxies), end(m_bodiesForProxies), &body);
2277 m_bodiesForProxies.erase(first, end(m_bodiesForProxies));
2280 void World::CreateAndDestroyProxies(
const StepConf& conf)
2282 for_each(begin(m_fixturesForProxies), end(m_fixturesForProxies), [&](Fixture *f) {
2283 CreateAndDestroyProxies(*f, conf);
2285 m_fixturesForProxies.clear();
2288 void World::CreateAndDestroyProxies(Fixture& fixture,
const StepConf& conf)
2290 const auto body = fixture.GetBody();
2291 const auto enabled = body->IsEnabled();
2293 const auto proxyCount = fixture.GetProxyCount();
2294 if (proxyCount == 0)
2298 CreateProxies(fixture, conf.aabbExtension);
2305 UnregisterForProxies(fixture);
2306 DestroyProxies(fixture);
2309 BodyAtty::EraseContacts(*body, [&](Contact& contact) {
2310 const auto fixtureA = contact.GetFixtureA();
2311 const auto fixtureB = contact.GetFixtureB();
2312 if ((fixtureA == &fixture) || (fixtureB == &fixture))
2314 Destroy(&contact, body);
2326 for_each(begin(m_bodiesForProxies), end(m_bodiesForProxies), [&](Body *b) {
2327 const auto xfm = b->GetTransformation();
2329 proxiesMoved += Synchronize(*b, xfm, xfm, conf.displaceMultiplier, conf.aabbExtension);
2331 m_bodiesForProxies.clear();
2332 return proxiesMoved;
2337 assert(body.GetWorld() ==
this);
2338 if (body.GetType() == type)
2345 throw WrongState(
"World::SetType: world is locked");
2348 BodyAtty::SetTypeFlags(body, type);
2349 body.ResetMassData();
2352 BodyAtty::EraseContacts(body, [&](Contact& contact) {
2353 Destroy(&contact, &body);
2361 const auto xfm2 = body.GetTransformation();
2362 assert(xfm1 == xfm2);
2364 RegisterForProxies(body);
2369 const auto fixtures = body.GetFixtures();
2370 for_each(begin(fixtures), end(fixtures), [&](Body::Fixtures::value_type& f) {
2371 InternalTouchProxies(
GetRef(f));
2376 Fixture* World::CreateFixture(Body& body,
const Shape& shape,
2377 const FixtureConf& def,
bool resetMassData)
2379 assert(body.GetWorld() ==
this);
2383 const auto minVertexRadius = GetMinVertexRadius();
2384 const auto maxVertexRadius = GetMaxVertexRadius();
2388 if (!(vr >= minVertexRadius))
2390 throw InvalidArgument(
"World::CreateFixture: vertex radius < min");
2392 if (!(vr <= maxVertexRadius))
2394 throw InvalidArgument(
"World::CreateFixture: vertex radius > max");
2401 throw WrongState(
"World::CreateFixture: world is locked");
2405 const auto fixture = FixtureAtty::Create(body, def, shape);
2406 BodyAtty::AddFixture(body, fixture);
2408 if (body.IsEnabled())
2410 RegisterForProxies(*fixture);
2414 if (fixture->GetDensity() > 0_kgpm2)
2416 BodyAtty::SetMassDataDirty(body);
2419 body.ResetMassData();
2430 bool World::Destroy(Fixture& fixture,
bool resetMassData)
2432 auto& body = *fixture.GetBody();
2433 assert(body.GetWorld() ==
this);
2437 throw WrongState(
"World::Destroy: world is locked");
2445 if (m_destructionListener)
2447 m_destructionListener->SayGoodbye(fixture);
2452 BodyAtty::EraseContacts(body, [&](Contact& contact) {
2453 const auto fixtureA = contact.GetFixtureA();
2454 const auto fixtureB = contact.GetFixtureB();
2455 if ((fixtureA == &fixture) || (fixtureB == &fixture))
2457 Destroy(&contact, &body);
2463 UnregisterForProxies(fixture);
2464 DestroyProxies(fixture);
2466 if (!BodyAtty::RemoveFixture(body, &fixture))
2473 BodyAtty::SetMassDataDirty(body);
2476 body.ResetMassData();
2482 void World::CreateProxies(Fixture& fixture,
Length aabbExtension)
2484 assert(fixture.GetProxyCount() == 0);
2486 const auto body = fixture.GetBody();
2487 const auto shape = fixture.GetShape();
2492 auto proxies = std::make_unique<FixtureProxy[]>(childCount);
2493 for (
auto childIndex = decltype(childCount){0}; childIndex < childCount; ++childIndex)
2495 const auto dp =
GetChild(shape, childIndex);
2500 const auto treeId = m_tree.CreateLeaf(fattenedAABB, DynamicTree::LeafData{
2501 body, &fixture, childIndex});
2502 RegisterForProcessing(treeId);
2503 proxies[childIndex] = FixtureProxy{treeId};
2506 FixtureAtty::SetProxies(fixture, std::move(proxies), childCount);
2509 void World::DestroyProxies(Fixture& fixture) noexcept
2511 const auto proxies = FixtureAtty::GetProxies(fixture);
2512 const auto childCount =
size(proxies);
2516 for (
auto i = childCount - 1; i < childCount; --i)
2518 const auto treeId = proxies[i].treeId;
2519 UnregisterForProcessing(treeId);
2520 m_tree.DestroyLeaf(treeId);
2523 FixtureAtty::ResetProxies(fixture);
2526 void World::TouchProxies(Fixture& fixture) noexcept
2528 assert(fixture.GetBody()->GetWorld() ==
this);
2529 InternalTouchProxies(fixture);
2532 void World::InternalTouchProxies(Fixture& fixture) noexcept
2534 const auto proxyCount = fixture.GetProxyCount();
2535 for (
auto i = decltype(proxyCount){0}; i < proxyCount; ++i)
2537 RegisterForProcessing(fixture.GetProxy(i).treeId);
2542 Transformation xfm1, Transformation xfm2,
2549 const auto displacement = multiplier * (xfm2.p - xfm1.p);
2550 const auto fixtures = body.GetFixtures();
2551 for_each(begin(fixtures), end(fixtures), [&](Body::Fixtures::value_type& f) {
2552 updatedCount += Synchronize(
GetRef(f), xfm1, xfm2, displacement, extension);
2554 return updatedCount;
2558 Transformation xfm1, Transformation xfm2,
2565 const auto shape = fixture.GetShape();
2566 const auto proxies = FixtureAtty::GetProxies(fixture);
2568 for (
auto& proxy: proxies)
2570 const auto treeId = proxy.treeId;
2574 if (!
Contains(m_tree.GetAABB(treeId), aabb))
2578 m_tree.UpdateLeaf(treeId, newAabb);
2579 RegisterForProcessing(treeId);
2584 return updatedCount;
2597 if (positionIterations == 0)
2602 return world.
Step(conf);
2607 const auto contacts = world.GetContacts();
2608 return static_cast<ContactCounter>(count_if(cbegin(contacts), cend(contacts),
2609 [&](
const World::Contacts::value_type &c) {
2610 return GetRef(std::get<Contact*>(c)).IsTouching();
2616 auto sum =
size_t{0};
2617 const auto bodies = world.GetBodies();
2618 for_each(cbegin(bodies), cend(bodies),
2619 [&](
const World::Bodies::value_type &body) {
2627 auto shapes = std::set<const void*>();
2628 const auto bodies = world.GetBodies();
2629 for_each(cbegin(bodies), cend(bodies), [&](
const World::Bodies::value_type &b) {
2630 const auto fixtures =
GetRef(b).GetFixtures();
2631 for_each(cbegin(fixtures), cend(fixtures), [&](
const Body::Fixtures::value_type& f) {
2635 return size(shapes);
2640 const auto bodies = world.GetBodies();
2641 return static_cast<BodyCounter>(count_if(cbegin(bodies), cend(bodies),
2642 [&](
const World::Bodies::value_type &b) {
2643 return GetRef(b).IsAwake(); }));
2650 const auto bodies = world.GetBodies();
2651 for_each(begin(bodies), end(bodies), [&](World::Bodies::value_type &b) {
2662 const auto bodies = world.GetBodies();
2663 for_each(begin(bodies), end(bodies), [&](World::Bodies::value_type &b) {
2670 const auto bodies = world.GetBodies();
2671 for_each(begin(bodies), end(bodies), [&](World::Bodies::value_type &b) {
2678 const auto bodies = world.GetBodies();
2679 auto found =
static_cast<decltype(bodies)::iterator_type::value_type
>(
nullptr);
2680 auto minLengthSquared = std::numeric_limits<Area>::infinity();
2681 for (
const auto& b: bodies)
2684 const auto bodyLoc = body.GetLocation();
2686 if (minLengthSquared > lengthSquared)
2688 minLengthSquared = lengthSquared;
2699 lhs.
maxIncImpulse = std::max(lhs.maxIncImpulse, rhs.maxIncImpulse);
2700 lhs.minSeparation = std::min(lhs.minSeparation, rhs.minSeparation);
2701 lhs.islandsSolved += rhs.solved;
2702 lhs.sumPosIters += rhs.positionIterations;
2703 lhs.sumVelIters += rhs.velocityIterations;
2704 lhs.bodiesSlept += rhs.bodiesSlept;