32 #include <type_traits>
46 auto flags = GetFlags(bd.type);
49 flags |= e_impenetrableFlag;
53 flags |= e_fixedRotationFlag;
57 flags |= e_autoSleepFlag;
61 if ((flags & e_velocityFlag) != 0)
68 if (!bd.allowSleep && ((flags & e_velocityFlag) != 0))
75 flags |= e_enabledFlag;
82 m_sweep{Position{bd.location, bd.angle}},
83 m_flags{GetFlags(bd)},
85 m_userData{bd.userData},
87 m_linearDamping{bd.linearDamping},
88 m_angularDamping{bd.angularDamping}
92 assert(
IsValid(bd.linearVelocity));
93 assert(
IsValid(bd.angularVelocity));
96 SetVelocity(Velocity{bd.linearVelocity, bd.angularVelocity});
98 SetUnderActiveTime(bd.underActiveTime);
101 Body::~Body() noexcept
103 assert(
empty(m_joints));
104 assert(
empty(m_contacts));
105 assert(
empty(m_fixtures));
110 WorldAtty::SetType(*m_world, *
this, type);
116 return WorldAtty::CreateFixture(*m_world, *
this, shape, def, resetMassData);
121 if (fixture->
GetBody() !=
this)
125 return WorldAtty::Destroy(*m_world, *fixture, resetMassData);
130 while (!
empty(m_fixtures))
132 const auto fixture =
GetPtr(m_fixtures.front());
148 UnsetMassDataDirty();
155 const auto mass = (massData.mass > 0_kg)?
Mass{massData.mass}: 1_kg;
156 m_invMass =
Real{1} / mass;
159 const auto localCenter = massData.center *
Real{m_invMass *
Kilogram};
167 m_invRotI =
Real{1} / I;
180 const auto deltaCenter = newCenter - oldCenter;
183 UnsetMassDataDirty();
190 throw WrongState(
"Body::SetMassData: world is locked");
198 const auto mass = (massData.
mass > 0_kg)?
Mass{massData.
mass}: 1_kg;
199 m_invMass =
Real{1} / mass;
207 m_invRotI =
Real{1} / I;
223 const auto deltaCenter = newCenter - oldCenter;
226 UnsetMassDataDirty();
231 if ((velocity.linear !=
LinearVelocity2{}) || (velocity.angular != 0_rpm))
238 ResetUnderActiveTime();
240 m_velocity = velocity;
248 if ((m_linearAcceleration == linear) && (m_angularAcceleration == angular))
254 if (!IsAccelerable())
264 if ((m_angularAcceleration < angular) ||
271 ResetUnderActiveTime();
275 m_linearAcceleration = linear;
276 m_angularAcceleration = angular;
285 std::for_each(cbegin(m_contacts), cend(m_contacts), [&](
KeyedContactPtr ci) {
286 std::get<Contact*>(ci)->FlagForUpdating();
298 throw WrongState(
"Body::SetTransform: world is locked");
302 SetTransformation(xfm);
306 WorldAtty::RegisterForProxies(*
GetWorld(), *
this);
318 throw WrongState(
"Body::SetEnabled: world is locked");
331 std::for_each(begin(m_fixtures), end(m_fixtures), [&](Fixtures::value_type &f) {
332 WorldAtty::RegisterForProxies(*m_world,
GetRef(f));
358 bool Body::Insert(
Joint* joint)
360 const auto bodyA = joint->
GetBodyA();
361 const auto bodyB = joint->
GetBodyB();
363 const auto other = (
this == bodyA)? bodyB: (
this == bodyB)? bodyA:
nullptr;
364 m_joints.push_back(std::make_pair(other, joint));
368 bool Body::Insert(
ContactKey key, Contact* contact)
372 const auto it = std::find_if(cbegin(m_contacts), cend(m_contacts), [&](
KeyedContactPtr ci) {
373 return std::get<Contact*>(ci) == contact;
375 assert(it == end(m_contacts));
376 if (it != end(m_contacts))
382 m_contacts.emplace_back(key, contact);
386 bool Body::Erase(
const Joint* joint)
388 const auto it = std::find_if(begin(m_joints), end(m_joints), [&](
KeyedJointPtr ji) {
389 return std::get<Joint*>(ji) == joint;
391 if (it != end(m_joints))
399 bool Body::Erase(
const Contact* contact)
401 const auto it = std::find_if(begin(m_contacts), end(m_contacts), [&](
KeyedContactPtr ci) {
402 return std::get<Contact*>(ci) == contact;
404 if (it != end(m_contacts))
406 m_contacts.erase(it);
412 void Body::ClearContacts()
417 void Body::ClearJoints()
427 if (!lhs.IsAccelerable() && !rhs.IsAccelerable())
433 const auto joints = lhs.GetJoints();
435 return (
std::get<0>(ji) == &rhs) && !(std::get<Joint*>(ji)->GetCollideConnected());
437 return it == end(joints);
444 const auto world = body->GetWorld();
445 const auto bodies = world->GetBodies();
447 const auto it = std::find_if(cbegin(bodies), cend(bodies), [&](
const Body *b) {
448 return b == body || ((void) ++i,
false);
450 if (it != end(bodies))
461 auto velocity = body.GetVelocity();
462 if (body.IsAccelerable())
465 velocity.
linear += h * body.GetLinearAcceleration();
466 velocity.angular += h * body.GetAngularAcceleration();
475 velocity.linear /=
Real{1 + h * body.GetLinearDamping()};
476 velocity.angular /=
Real{1 + h * body.GetAngularDamping()};
484 const auto translation = h * velocity.
linear;
486 if (lsquared >
Square(conf.maxTranslation))
489 const auto ratio = conf.maxTranslation /
sqrt(lsquared);
490 velocity.linear *= ratio;
493 const auto absRotation =
abs(h * velocity.angular);
494 if (absRotation > conf.maxRotation)
497 const auto ratio = conf.maxRotation / absRotation;
498 velocity.angular *= ratio;
506 const auto& fixtures = body.GetFixtures();
507 return size(fixtures);
513 const auto p = xfm.
p - worldPoint;
514 const auto c =
cos(amount);
515 const auto s =
sin(amount);
516 const auto x =
GetX(p) * c -
GetY(p) * s;
517 const auto y =
GetX(p) * s +
GetY(p) * c;
518 const auto pos =
Length2{x, y} + worldPoint;
519 const auto angle =
GetAngle(xfm.q) + amount;
537 const auto mass =
GetMass(body);
538 const auto delta = axis - location;
540 const auto dir = delta * invRadius;
541 return Force2{dir * mass *
Square(magnitudeOfVelocity) * invRadius};
551 const auto world = body.GetWorld();
552 const auto bodies = world->GetBodies();
553 for (
auto jt = begin(bodies); jt != end(bodies); jt = std::next(jt))
555 const auto& b2 = *(*jt);
573 const auto orderedMass = std::minmax(m1, m2);