From 4347e398ca94db9e9c96b83e2353e9f1a36363fb Mon Sep 17 00:00:00 2001 From: louis-langholtz Date: Sun, 30 Jul 2017 09:20:00 -0600 Subject: [PATCH] Fixes issue #8: Refactor joint definition structures to provide builder interface. --- PlayRho/Common/Vector.cpp | 9 + PlayRho/Common/Vector.hpp | 14 ++ PlayRho/Dynamics/Joints/DistanceJoint.cpp | 30 ++- PlayRho/Dynamics/Joints/DistanceJoint.hpp | 37 +++- PlayRho/Dynamics/Joints/FrictionJoint.cpp | 2 +- PlayRho/Dynamics/Joints/FrictionJoint.hpp | 6 +- PlayRho/Dynamics/Joints/GearJoint.cpp | 2 +- PlayRho/Dynamics/Joints/GearJoint.hpp | 6 +- PlayRho/Dynamics/Joints/Joint.cpp | 215 +++++++-------------- PlayRho/Dynamics/Joints/Joint.hpp | 139 +++++++------ PlayRho/Dynamics/Joints/MotorJoint.cpp | 2 +- PlayRho/Dynamics/Joints/MotorJoint.hpp | 6 +- PlayRho/Dynamics/Joints/PrismaticJoint.cpp | 2 +- PlayRho/Dynamics/Joints/PrismaticJoint.hpp | 6 +- PlayRho/Dynamics/Joints/PulleyJoint.cpp | 10 +- PlayRho/Dynamics/Joints/PulleyJoint.hpp | 15 +- PlayRho/Dynamics/Joints/RevoluteJoint.cpp | 6 +- PlayRho/Dynamics/Joints/RevoluteJoint.hpp | 9 +- PlayRho/Dynamics/Joints/RopeJoint.hpp | 9 +- PlayRho/Dynamics/Joints/WeldJoint.cpp | 5 +- PlayRho/Dynamics/Joints/WeldJoint.hpp | 30 +-- PlayRho/Dynamics/Joints/WheelJoint.cpp | 2 +- PlayRho/Dynamics/Joints/WheelJoint.hpp | 6 +- Testbed/Tests/Dominos.hpp | 38 ++-- Testbed/Tests/Pulleys.hpp | 4 +- Testbed/Tests/TheoJansen.hpp | 21 +- UnitTests/GearJoint.cpp | 4 +- UnitTests/World.cpp | 4 +- 28 files changed, 330 insertions(+), 309 deletions(-) create mode 100644 PlayRho/Common/Vector.cpp create mode 100644 PlayRho/Common/Vector.hpp diff --git a/PlayRho/Common/Vector.cpp b/PlayRho/Common/Vector.cpp new file mode 100644 index 0000000000..eab45e51ec --- /dev/null +++ b/PlayRho/Common/Vector.cpp @@ -0,0 +1,9 @@ +// +// Vector.cpp +// PlayRho +// +// Created by Louis D. Langholtz on 7/29/17. +// +// + +#include "Vector.hpp" diff --git a/PlayRho/Common/Vector.hpp b/PlayRho/Common/Vector.hpp new file mode 100644 index 0000000000..aea8188911 --- /dev/null +++ b/PlayRho/Common/Vector.hpp @@ -0,0 +1,14 @@ +// +// Vector.hpp +// PlayRho +// +// Created by Louis D. Langholtz on 7/29/17. +// +// + +#ifndef Vector_hpp +#define Vector_hpp + +#include + +#endif /* Vector_hpp */ diff --git a/PlayRho/Dynamics/Joints/DistanceJoint.cpp b/PlayRho/Dynamics/Joints/DistanceJoint.cpp index c0988444ca..b67da96ef2 100644 --- a/PlayRho/Dynamics/Joints/DistanceJoint.cpp +++ b/PlayRho/Dynamics/Joints/DistanceJoint.cpp @@ -31,7 +31,7 @@ using namespace playrho; // x2 = x1 + h * v2 // 1-D mass-damper-spring system -// m (v2 - v1) + h * d * v2 + h * k * +// m (v2 - v1) + h * d * v2 + h * k * // C = norm(p2 - p1) - L // u = (p2 - p1) / norm(p2 - p1) @@ -41,12 +41,11 @@ using namespace playrho; // = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 DistanceJointDef::DistanceJointDef(NonNull bA, NonNull bB, - const Length2D anchor1, const Length2D anchor2, - NonNegative freq, Real damp) noexcept: - JointDef{JointType::Distance, bA, bB}, - localAnchorA{GetLocalPoint(*bA, anchor1)}, localAnchorB{GetLocalPoint(*bB, anchor2)}, - length{GetLength(anchor2 - anchor1)}, - frequency{freq}, dampingRatio{damp} + Length2D anchor1, Length2D anchor2) noexcept + : super{super{JointType::Distance}.UseBodyA(bA).UseBodyB(bB)}, + localAnchorA{GetLocalPoint(*bA, anchor1)}, + localAnchorB{GetLocalPoint(*bB, anchor2)}, + length{GetLength(anchor2 - anchor1)} { // Intentionally empty. } @@ -77,7 +76,7 @@ DistanceJoint::DistanceJoint(const DistanceJointDef& def): void DistanceJoint::InitVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step, - const ConstraintSolverConf& conf) + const ConstraintSolverConf&) { auto& bodyConstraintA = At(bodies, static_cast(GetBodyA())); auto& bodyConstraintB = At(bodies, GetBodyB()); @@ -102,11 +101,7 @@ void DistanceJoint::InitVelocityConstraints(BodyConstraintsMap& bodies, // Handle singularity. auto length = Length{0}; - m_u = GetUnitVector(deltaLocation, length); - if (length <= conf.linearSlop) - { - m_u = UnitVec2::GetZero(); - } + m_u = GetUnitVector(deltaLocation, length, UnitVec2::GetZero()); const auto crAu = Length{Cross(m_rA, m_u)} / Radian; const auto crBu = Length{Cross(m_rB, m_u)} / Radian; @@ -199,11 +194,12 @@ bool DistanceJoint::SolveVelocityConstraints(BodyConstraintsMap& bodies, const S bodyConstraintA->SetVelocity(velA); bodyConstraintB->SetVelocity(velB); - + return impulse == Momentum{0}; } -bool DistanceJoint::SolvePositionConstraints(BodyConstraintsMap& bodies, const ConstraintSolverConf& conf) const +bool DistanceJoint::SolvePositionConstraints(BodyConstraintsMap& bodies, + const ConstraintSolverConf& conf) const { if (m_frequency > Frequency{0}) { @@ -228,7 +224,7 @@ bool DistanceJoint::SolvePositionConstraints(BodyConstraintsMap& bodies, const C const auto rA = Length2D{Rotate(m_localAnchorA - bodyConstraintA->GetLocalCenter(), qA)}; const auto rB = Length2D{Rotate(m_localAnchorB - bodyConstraintB->GetLocalCenter(), qB)}; const auto relLoc = Length2D{(posB.linear + rB) - (posA.linear + rA)}; - + auto length = Length{0}; const auto u = GetUnitVector(relLoc, length); const auto deltaLength = length - m_length; @@ -270,7 +266,7 @@ Torque DistanceJoint::GetReactionTorque(Frequency inv_dt) const DistanceJointDef playrho::GetDistanceJointDef(const DistanceJoint& joint) noexcept { auto def = DistanceJointDef{}; - + Set(def, joint); def.localAnchorA = joint.GetLocalAnchorA(); diff --git a/PlayRho/Dynamics/Joints/DistanceJoint.hpp b/PlayRho/Dynamics/Joints/DistanceJoint.hpp index 5e63340e9a..3b1dbe3802 100644 --- a/PlayRho/Dynamics/Joints/DistanceJoint.hpp +++ b/PlayRho/Dynamics/Joints/DistanceJoint.hpp @@ -31,19 +31,37 @@ namespace playrho { /// the initial configuration can violate the constraint slightly. This helps when // saving and loading a game. /// @warning Do not use a zero or short length. -struct DistanceJointDef : public JointDef +struct DistanceJointDef : public JointBuilder { - constexpr DistanceJointDef() noexcept: JointDef{JointType::Distance} {} + using super = JointBuilder; + + constexpr DistanceJointDef() noexcept: super{JointType::Distance} {} DistanceJointDef(const DistanceJointDef& copy) = default; /// @brief Initializing constructor. /// @details Initialize the bodies, anchors, and length using the world anchors. DistanceJointDef(NonNull bodyA, NonNull bodyB, - const Length2D anchorA = Length2D(0, 0), - const Length2D anchorB = Length2D(0, 0), - NonNegative freq = NonNegative{0}, - Real damp = 0) noexcept; + Length2D anchorA = Length2D(0, 0), + Length2D anchorB = Length2D(0, 0)) noexcept; + + constexpr DistanceJointDef& UseLength(Length v) noexcept + { + length = v; + return *this; + } + + constexpr DistanceJointDef& UseFrequency(NonNegative v) noexcept + { + frequency = v; + return *this; + } + + constexpr DistanceJointDef& UseDampingRatio(Real v) noexcept + { + dampingRatio = v; + return *this; + } /// @brief Local anchor point relative to bodyA's origin. Length2D localAnchorA = Length2D(0, 0); @@ -108,7 +126,8 @@ class DistanceJoint : public Joint private: - void InitVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step, const ConstraintSolverConf& conf) override; + void InitVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step, const + ConstraintSolverConf&) override; bool SolveVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step) override; bool SolvePositionConstraints(BodyConstraintsMap& bodies, const ConstraintSolverConf& conf) const override; @@ -118,9 +137,11 @@ class DistanceJoint : public Joint NonNegative m_frequency; Real m_dampingRatio; + // Solver shared + Momentum m_impulse = Momentum{0}; + // Solver temp InvMass m_invGamma; - Momentum m_impulse = Momentum{0}; LinearVelocity m_bias; Mass m_mass; UnitVec2 m_u; diff --git a/PlayRho/Dynamics/Joints/FrictionJoint.cpp b/PlayRho/Dynamics/Joints/FrictionJoint.cpp index 71b129c1c3..abc126b668 100644 --- a/PlayRho/Dynamics/Joints/FrictionJoint.cpp +++ b/PlayRho/Dynamics/Joints/FrictionJoint.cpp @@ -37,7 +37,7 @@ using namespace playrho; // K = invI1 + invI2 FrictionJointDef::FrictionJointDef(Body* bA, Body* bB, const Length2D anchor) noexcept: - JointDef{JointType::Friction, bA, bB}, + super{super{JointType::Friction}.UseBodyA(bA).UseBodyB(bB)}, localAnchorA{GetLocalPoint(*bA, anchor)}, localAnchorB{GetLocalPoint(*bB, anchor)} { diff --git a/PlayRho/Dynamics/Joints/FrictionJoint.hpp b/PlayRho/Dynamics/Joints/FrictionJoint.hpp index 28d3edcb0b..5182d1c7c0 100644 --- a/PlayRho/Dynamics/Joints/FrictionJoint.hpp +++ b/PlayRho/Dynamics/Joints/FrictionJoint.hpp @@ -25,9 +25,11 @@ namespace playrho { /// @brief Friction joint definition. -struct FrictionJointDef : public JointDef +struct FrictionJointDef : public JointBuilder { - constexpr FrictionJointDef() noexcept: JointDef(JointType::Friction) {} + using super = JointBuilder; + + constexpr FrictionJointDef() noexcept: super{JointType::Friction} {} /// @brief Initializing constructor. /// @details Initialize the bodies, anchors, axis, and reference angle using the world diff --git a/PlayRho/Dynamics/Joints/GearJoint.cpp b/PlayRho/Dynamics/Joints/GearJoint.cpp index 8c6431f330..ec6ac866e2 100644 --- a/PlayRho/Dynamics/Joints/GearJoint.cpp +++ b/PlayRho/Dynamics/Joints/GearJoint.cpp @@ -47,7 +47,7 @@ using namespace playrho; // K = J * invM * JT = invMass + invI * cross(r, ug)^2 GearJoint::GearJoint(const GearJointDef& def): - Joint(JointDef(def).UseBodyA(def.joint1->GetBodyB()).UseBodyB(def.joint2->GetBodyB())), + Joint(def), m_joint1(def.joint1), m_joint2(def.joint2), m_typeA(def.joint1->GetType()), diff --git a/PlayRho/Dynamics/Joints/GearJoint.hpp b/PlayRho/Dynamics/Joints/GearJoint.hpp index 03f4269988..7116dcd3d9 100644 --- a/PlayRho/Dynamics/Joints/GearJoint.hpp +++ b/PlayRho/Dynamics/Joints/GearJoint.hpp @@ -28,10 +28,12 @@ namespace playrho { /// @brief Gear joint definition. /// @details This definition requires two existing /// revolute or prismatic joints (any combination will work). -struct GearJointDef : public JointDef +struct GearJointDef : public JointBuilder { + using super = JointBuilder; + GearJointDef(NonNull j1, NonNull j2) noexcept: - JointDef{JointDef(JointType::Gear).UseBodyA(j1->GetBodyB()).UseBodyB(j2->GetBodyB())}, + super{super{JointType::Gear}.UseBodyA(j1->GetBodyB()).UseBodyB(j2->GetBodyB())}, joint1{j1}, joint2{j2} { // Intentionally empty. diff --git a/PlayRho/Dynamics/Joints/Joint.cpp b/PlayRho/Dynamics/Joints/Joint.cpp index 938dc7cf66..3fb55fd3c0 100644 --- a/PlayRho/Dynamics/Joints/Joint.cpp +++ b/PlayRho/Dynamics/Joints/Joint.cpp @@ -1,21 +1,23 @@ /* -* Original work Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* Modified work Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Original work Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * Modified work Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #include #include @@ -39,140 +41,37 @@ namespace playrho { -namespace -{ - inline DistanceJoint* Create(const DistanceJointDef& def) - { - if (DistanceJoint::IsOkay(def)) - { - return new DistanceJoint(def); - } - return nullptr; - } - - inline MouseJoint* Create(const MouseJointDef& def) - { - if (MouseJoint::IsOkay(static_cast(def))) - { - return new MouseJoint(def); - } - return nullptr; - } - - inline PrismaticJoint* Create(const PrismaticJointDef& def) - { - if (PrismaticJoint::IsOkay(def)) - { - return new PrismaticJoint(def); - } - return nullptr; - } - - inline RevoluteJoint* Create(const RevoluteJointDef& def) - { - if (RevoluteJoint::IsOkay(def)) - { - return new RevoluteJoint(def); - } - return nullptr; - } - - inline PulleyJoint* Create(const PulleyJointDef& def) - { - if (PulleyJoint::IsOkay(def)) - { - return new PulleyJoint(def); - } - return nullptr; - } - - inline GearJoint* Create(const GearJointDef& def) - { - if (GearJoint::IsOkay(def)) - { - return new GearJoint(def); - } - return nullptr; - } - - inline WheelJoint* Create(const WheelJointDef& def) - { - if (WheelJoint::IsOkay(def)) - { - return new WheelJoint(def); - } - return nullptr; - } - - inline WeldJoint* Create(const WeldJointDef& def) - { - if (WeldJoint::IsOkay(def)) - { - return new WeldJoint(def); - } - return nullptr; - } - - inline FrictionJoint* Create(const FrictionJointDef& def) - { - if (FrictionJoint::IsOkay(def)) - { - return new FrictionJoint(def); - } - return nullptr; - } - - inline RopeJoint* Create(const RopeJointDef& def) - { - if (RopeJoint::IsOkay(def)) - { - return new RopeJoint(def); - } - return nullptr; - } - - inline MotorJoint* Create(const MotorJointDef& def) - { - if (MotorJoint::IsOkay(def)) - { - return new MotorJoint(def); - } - return nullptr; - } - -} // anonymous namespace - Joint* Joint::Create(const JointDef& def) { switch (def.type) { - case JointType::Distance: - return playrho::Create(static_cast(def)); - case JointType::Mouse: - return playrho::Create(static_cast(def)); - case JointType::Prismatic: - return playrho::Create(static_cast(def)); - case JointType::Revolute: - return playrho::Create(static_cast(def)); - case JointType::Pulley: - return playrho::Create(static_cast(def)); - case JointType::Gear: - return playrho::Create(static_cast(def)); - case JointType::Wheel: - return playrho::Create(static_cast(def)); - case JointType::Weld: - return playrho::Create(static_cast(def)); - case JointType::Friction: - return playrho::Create(static_cast(def)); - case JointType::Rope: - return playrho::Create(static_cast(def)); - case JointType::Motor: - return playrho::Create(static_cast(def)); - case JointType::Unknown: - assert(false); - break; - default: - break; + case JointType::Distance: + return Create(static_cast(def)); + case JointType::Mouse: + return Create(static_cast(def)); + case JointType::Prismatic: + return Create(static_cast(def)); + case JointType::Revolute: + return Create(static_cast(def)); + case JointType::Pulley: + return Create(static_cast(def)); + case JointType::Gear: + return Create(static_cast(def)); + case JointType::Wheel: + return Create(static_cast(def)); + case JointType::Weld: + return Create(static_cast(def)); + case JointType::Friction: + return Create(static_cast(def)); + case JointType::Rope: + return Create(static_cast(def)); + case JointType::Motor: + return Create(static_cast(def)); + case JointType::Unknown: + assert(false); + break; + default: + break; } return nullptr; } @@ -202,6 +101,8 @@ void SetAwake(Joint& j) noexcept j.GetBodyB()->SetAwake(); } +// Free functions... + JointCounter GetWorldIndex(const Joint* joint) { if (joint) @@ -233,4 +134,24 @@ void Set(JointDef& def, const Joint& joint) noexcept def.collideConnected = joint.GetCollideConnected(); } +BodyConstraintPtr& At(std::vector& container, const Body* key) +{ + auto last = std::end(container); + auto first = std::begin(container); + first = std::lower_bound(first, last, key, [](const BodyConstraintPair &a, const Body* b){ + return a.first < b; + }); + if (first == last || key != (*first).first) + { + throw std::out_of_range{"invalid key"}; + } + return (*first).second; +} + +BodyConstraintPtr& At(std::unordered_map& container, + const Body* key) +{ + return container.at(key); +} + } // namespace playrho diff --git a/PlayRho/Dynamics/Joints/Joint.hpp b/PlayRho/Dynamics/Joints/Joint.hpp index 6d83912969..5b931af6f0 100644 --- a/PlayRho/Dynamics/Joints/Joint.hpp +++ b/PlayRho/Dynamics/Joints/Joint.hpp @@ -1,21 +1,23 @@ /* -* Original work Copyright (c) 2006-2007 Erin Catto http://www.box2d.org -* Modified work Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho -* -* This software is provided 'as-is', without any express or implied -* warranty. In no event will the authors be held liable for any damages -* arising from the use of this software. -* Permission is granted to anyone to use this software for any purpose, -* including commercial applications, and to alter it and redistribute it -* freely, subject to the following restrictions: -* 1. The origin of this software must not be misrepresented; you must not -* claim that you wrote the original software. If you use this software -* in a product, an acknowledgment in the product documentation would be -* appreciated but is not required. -* 2. Altered source versions must be plainly marked as such, and must not be -* misrepresented as being the original software. -* 3. This notice may not be removed or altered from any source distribution. -*/ + * Original work Copyright (c) 2006-2007 Erin Catto http://www.box2d.org + * Modified work Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho + * + * This software is provided 'as-is', without any express or implied + * warranty. In no event will the authors be held liable for any damages + * arising from the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ #ifndef B2_JOINT_H #define B2_JOINT_H @@ -46,7 +48,7 @@ using BodyConstraintsMap = std::vector using BodyConstraintsMap = std::unordered_map; #endif -enum class JointType: std::uint8_t +enum class JointType : std::uint8_t { Unknown, Revolute, @@ -71,24 +73,13 @@ struct JointDef /// Deleted default constructor for abstract base class. JointDef() = delete; // deleted to prevent direct instantiation. - constexpr JointDef(JointType t) noexcept: - type{t} - { - // Intentionally empty. - } - - constexpr JointDef(JointType t, Body* bA, Body* bB, bool cc = false, - void* u = nullptr) noexcept: - type{t}, bodyA{bA}, bodyB{bB}, collideConnected{cc}, userData{u} + constexpr JointDef(JointType t) noexcept : type{t} { // Intentionally empty. } - constexpr JointDef& UseBodyA(Body* body) noexcept { bodyA = body; return *this; } - constexpr JointDef& UseBodyB(Body* body) noexcept { bodyB = body; return *this; } - /// The joint type is set automatically for concrete joint types. - const JointType type; + JointType type; /// @brief First attached body. Body* bodyA = nullptr; @@ -98,11 +89,47 @@ struct JointDef /// Set this flag to true if the attached bodies should collide. bool collideConnected = false; - + /// Use this to attach application specific data to your joints. void* userData = nullptr; }; +template +struct JointBuilder : JointDef +{ + using value_type = T; + using reference = value_type&; + + constexpr JointBuilder(JointType t) noexcept : JointDef{t} + { + // Intentionally empty. + } + + constexpr reference UseBodyA(Body* b) noexcept + { + bodyA = b; + return static_cast(*this); + } + + constexpr reference UseBodyB(Body* b) noexcept + { + bodyB = b; + return static_cast(*this); + } + + constexpr reference UseCollideConnected(bool v) noexcept + { + collideConnected = v; + return static_cast(*this); + } + + constexpr reference UseUserData(void* v) noexcept + { + userData = v; + return static_cast(*this); + } +}; + /// @brief Base joint class. /// @details Joints are used to constraint two bodies together in various fashions. /// Some joints also feature limits and motors. @@ -116,7 +143,7 @@ class Joint e_atUpperLimit, e_equalLimits }; - + static bool IsOkay(const JointDef& def) noexcept; /// @brief Gets the type of the concrete joint. @@ -158,24 +185,34 @@ class Joint protected: Joint(const JointDef& def); - + private: friend class JointAtty; /// Flags type data type. using FlagsType = std::uint8_t; - + /// @brief Flags stored in m_flags enum Flag: FlagsType { // Used when crawling contact graph when forming islands. e_islandFlag = 0x01, - + e_collideConnectedFlag = 0x02 }; - + static constexpr FlagsType GetFlags(const JointDef& def) noexcept; + template + static OUT_TYPE* Create(IN_TYPE def) + { + if (OUT_TYPE::IsOkay(def)) + { + return new OUT_TYPE(def); + } + return nullptr; + } + static Joint* Create(const JointDef& def); /// Destroys the given joint. @@ -195,8 +232,9 @@ class Joint virtual bool SolveVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step) = 0; // This returns true if the position errors are within tolerance. - virtual bool SolvePositionConstraints(BodyConstraintsMap& bodies, const ConstraintSolverConf& conf) const = 0; - + virtual bool SolvePositionConstraints(BodyConstraintsMap& bodies, + const ConstraintSolverConf& conf) const = 0; + bool IsIslanded() const noexcept; void SetIslanded() noexcept; void UnsetIslanded() noexcept; @@ -271,7 +309,7 @@ inline void Joint::UnsetIslanded() noexcept } // Free functions... - + /// @brief Short-cut function to determine if both bodies are enabled. bool IsEnabled(const Joint& j) noexcept; @@ -282,25 +320,10 @@ JointCounter GetWorldIndex(const Joint* joint); void Set(JointDef& def, const Joint& joint) noexcept; -inline BodyConstraintPtr& At(std::vector& container, const Body* key) -{ - auto last = std::end(container); - auto first = std::begin(container); - first = std::lower_bound(first, last, key, [](const BodyConstraintPair &a, const Body* b){ - return a.first < b; - }); - if (first == last || key != (*first).first) - { - throw std::out_of_range{"invalid key"}; - } - return (*first).second; -} +BodyConstraintPtr& At(std::vector& container, const Body* key); -inline BodyConstraintPtr& At(std::unordered_map& container, - const Body* key) -{ - return container.at(key); -} +BodyConstraintPtr& At(std::unordered_map& container, + const Body* key); } // namespace playrho diff --git a/PlayRho/Dynamics/Joints/MotorJoint.cpp b/PlayRho/Dynamics/Joints/MotorJoint.cpp index d73300ef2c..751ca011da 100644 --- a/PlayRho/Dynamics/Joints/MotorJoint.cpp +++ b/PlayRho/Dynamics/Joints/MotorJoint.cpp @@ -37,7 +37,7 @@ using namespace playrho; // K = invI1 + invI2 MotorJointDef::MotorJointDef(NonNull bA, NonNull bB) noexcept: - JointDef{JointType::Motor, bA, bB}, + super{super{JointType::Motor}.UseBodyA(bA).UseBodyB(bB)}, linearOffset{GetLocalPoint(*bA, bB->GetLocation())}, angularOffset{bB->GetAngle() - bA->GetAngle()} { diff --git a/PlayRho/Dynamics/Joints/MotorJoint.hpp b/PlayRho/Dynamics/Joints/MotorJoint.hpp index c947ff551a..51c8778502 100644 --- a/PlayRho/Dynamics/Joints/MotorJoint.hpp +++ b/PlayRho/Dynamics/Joints/MotorJoint.hpp @@ -26,9 +26,11 @@ namespace playrho { /// Motor joint definition. -struct MotorJointDef : public JointDef +struct MotorJointDef : public JointBuilder { - constexpr MotorJointDef() noexcept: JointDef(JointType::Motor) {} + using super = JointBuilder; + + constexpr MotorJointDef() noexcept: super{JointType::Motor} {} /// Initialize the bodies and offsets using the current transforms. MotorJointDef(NonNull bodyA, NonNull bodyB) noexcept; diff --git a/PlayRho/Dynamics/Joints/PrismaticJoint.cpp b/PlayRho/Dynamics/Joints/PrismaticJoint.cpp index 8a084edfb9..8bc1190397 100644 --- a/PlayRho/Dynamics/Joints/PrismaticJoint.cpp +++ b/PlayRho/Dynamics/Joints/PrismaticJoint.cpp @@ -94,7 +94,7 @@ using namespace playrho; PrismaticJointDef::PrismaticJointDef(NonNull bA, NonNull bB, const Length2D anchor, const UnitVec2 axis) noexcept: - JointDef{JointType::Prismatic, bA, bB}, + super{super{JointType::Prismatic}.UseBodyA(bA).UseBodyB(bB)}, localAnchorA{GetLocalPoint(*bA, anchor)}, localAnchorB{GetLocalPoint(*bB, anchor)}, localAxisA{GetLocalVector(*bA, axis)}, diff --git a/PlayRho/Dynamics/Joints/PrismaticJoint.hpp b/PlayRho/Dynamics/Joints/PrismaticJoint.hpp index 2fdbbc4dc7..f0526603d9 100644 --- a/PlayRho/Dynamics/Joints/PrismaticJoint.hpp +++ b/PlayRho/Dynamics/Joints/PrismaticJoint.hpp @@ -32,9 +32,11 @@ namespace playrho { /// can violate the constraint slightly. The joint translation is zero /// when the local anchor points coincide in world space. Using local /// anchors and a local axis helps when saving and loading a game. -struct PrismaticJointDef : public JointDef +struct PrismaticJointDef : public JointBuilder { - constexpr PrismaticJointDef() noexcept: JointDef(JointType::Prismatic) {} + using super = JointBuilder; + + constexpr PrismaticJointDef() noexcept: super{JointType::Prismatic} {} PrismaticJointDef(const PrismaticJointDef& copy) = default; diff --git a/PlayRho/Dynamics/Joints/PulleyJoint.cpp b/PlayRho/Dynamics/Joints/PulleyJoint.cpp index 9e1310da34..caeb089b56 100644 --- a/PlayRho/Dynamics/Joints/PulleyJoint.cpp +++ b/PlayRho/Dynamics/Joints/PulleyJoint.cpp @@ -39,18 +39,16 @@ using namespace playrho; PulleyJointDef::PulleyJointDef(NonNull bA, NonNull bB, const Length2D groundA, const Length2D groundB, - const Length2D anchorA, const Length2D anchorB, - Real r): - JointDef{JointType::Pulley, bA, bB, true}, + const Length2D anchorA, const Length2D anchorB): + super{super{JointType::Pulley}.UseBodyA(bA).UseBodyB(bB).UseCollideConnected(true)}, groundAnchorA{groundA}, groundAnchorB{groundB}, localAnchorA{GetLocalPoint(*bA, anchorA)}, localAnchorB{GetLocalPoint(*bB, anchorB)}, lengthA{GetLength(anchorA - groundA)}, - lengthB{GetLength(anchorB - groundB)}, - ratio{r} + lengthB{GetLength(anchorB - groundB)} { - assert((r > 0) && !almost_zero(r)); + // Intentionally empty. } PulleyJoint::PulleyJoint(const PulleyJointDef& def): diff --git a/PlayRho/Dynamics/Joints/PulleyJoint.hpp b/PlayRho/Dynamics/Joints/PulleyJoint.hpp index 05cc2174ed..bc18d42481 100644 --- a/PlayRho/Dynamics/Joints/PulleyJoint.hpp +++ b/PlayRho/Dynamics/Joints/PulleyJoint.hpp @@ -27,9 +27,11 @@ namespace playrho { /// @brief Pulley joint definition. /// @details This requires two ground anchors, two dynamic body anchor points, and a pulley ratio. -struct PulleyJointDef : public JointDef +struct PulleyJointDef : public JointBuilder { - PulleyJointDef() noexcept: JointDef(JointType::Pulley) + using super = JointBuilder; + + PulleyJointDef() noexcept: super{JointType::Pulley} { collideConnected = true; } @@ -37,8 +39,13 @@ struct PulleyJointDef : public JointDef /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. PulleyJointDef(NonNull bodyA, NonNull bodyB, const Length2D groundAnchorA, const Length2D groundAnchorB, - const Length2D anchorA, const Length2D anchorB, - Real ratio); + const Length2D anchorA, const Length2D anchorB); + + PulleyJointDef& UseRatio(Real v) noexcept + { + ratio = v; + return *this; + } /// The first ground anchor in world coordinates. This point never moves. Length2D groundAnchorA = Length2D{Real(-1) * Meter, Real(1) * Meter}; diff --git a/PlayRho/Dynamics/Joints/RevoluteJoint.cpp b/PlayRho/Dynamics/Joints/RevoluteJoint.cpp index b99ce87946..19d9d2f88d 100644 --- a/PlayRho/Dynamics/Joints/RevoluteJoint.cpp +++ b/PlayRho/Dynamics/Joints/RevoluteJoint.cpp @@ -38,9 +38,9 @@ using namespace playrho; // J = [0 0 -1 0 0 1] // K = invI1 + invI2 -RevoluteJointDef::RevoluteJointDef(NonNull bA, NonNull bB, const Length2D anchor, - bool cc) noexcept: - JointDef{JointType::Revolute, bA, bB, cc}, +RevoluteJointDef::RevoluteJointDef(NonNull bA, NonNull bB, + const Length2D anchor) noexcept: + super{super{JointType::Revolute}.UseBodyA(bA).UseBodyB(bB)}, localAnchorA{GetLocalPoint(*bA, anchor)}, localAnchorB{GetLocalPoint(*bB, anchor)}, referenceAngle{bB->GetAngle() - bA->GetAngle()} diff --git a/PlayRho/Dynamics/Joints/RevoluteJoint.hpp b/PlayRho/Dynamics/Joints/RevoluteJoint.hpp index b32f65ad0b..e0ac3abf0a 100644 --- a/PlayRho/Dynamics/Joints/RevoluteJoint.hpp +++ b/PlayRho/Dynamics/Joints/RevoluteJoint.hpp @@ -37,13 +37,14 @@ namespace playrho { /// 1. you might not know where the center of mass will be; /// 2. if you add/remove shapes from a body and recompute the mass, /// the joints will be broken. -struct RevoluteJointDef : public JointDef +struct RevoluteJointDef : public JointBuilder { - constexpr RevoluteJointDef() noexcept: JointDef{JointType::Revolute} {} + using super = JointBuilder; + + constexpr RevoluteJointDef() noexcept: super{JointType::Revolute} {} /// @brief Initialize the bodies, anchors, and reference angle using a world anchor point. - RevoluteJointDef(NonNull bodyA, NonNull bodyB, const Length2D anchor, - bool cc = false) noexcept; + RevoluteJointDef(NonNull bodyA, NonNull bodyB, const Length2D anchor) noexcept; /// @brief Local anchor point relative to bodyA's origin. Length2D localAnchorA = Length2D(0, 0); diff --git a/PlayRho/Dynamics/Joints/RopeJoint.hpp b/PlayRho/Dynamics/Joints/RopeJoint.hpp index f2c35fdf5d..da9a12ff91 100644 --- a/PlayRho/Dynamics/Joints/RopeJoint.hpp +++ b/PlayRho/Dynamics/Joints/RopeJoint.hpp @@ -28,11 +28,14 @@ namespace playrho { /// @details This requires two body anchor points and a maximum lengths. /// @note By default the connected objects will not collide. /// @see collideConnected in JointDef. -struct RopeJointDef : public JointDef +struct RopeJointDef : public JointBuilder { - constexpr RopeJointDef() noexcept: JointDef(JointType::Rope) {} + using super = JointBuilder; - constexpr RopeJointDef(Body* bodyA, Body* bodyB) noexcept: JointDef(JointType::Rope, bodyA, bodyB) {} + constexpr RopeJointDef() noexcept: super{JointType::Rope} {} + + constexpr RopeJointDef(Body* bodyA, Body* bodyB) noexcept: + super{super{JointType::Rope}.UseBodyA(bodyA).UseBodyB(bodyB)} {} /// The local anchor point relative to bodyA's origin. Length2D localAnchorA = Length2D{Real(-1) * Meter, Real(0) * Meter}; diff --git a/PlayRho/Dynamics/Joints/WeldJoint.cpp b/PlayRho/Dynamics/Joints/WeldJoint.cpp index bff9d2b29a..cb163c4cd4 100644 --- a/PlayRho/Dynamics/Joints/WeldJoint.cpp +++ b/PlayRho/Dynamics/Joints/WeldJoint.cpp @@ -40,7 +40,7 @@ using namespace playrho; // K = invI1 + invI2 WeldJointDef::WeldJointDef(NonNull bA, NonNull bB, const Length2D anchor) noexcept: - JointDef{JointType::Weld, bA, bB}, + super{super{JointType::Weld}.UseBodyA(bA).UseBodyB(bB)}, localAnchorA{GetLocalPoint(*bA, anchor)}, localAnchorB{GetLocalPoint(*bB, anchor)}, referenceAngle{bB->GetAngle() - bA->GetAngle()} @@ -59,7 +59,8 @@ WeldJoint::WeldJoint(const WeldJointDef& def): // Intentionally empty. } -void WeldJoint::InitVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step, const ConstraintSolverConf&) +void WeldJoint::InitVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step, + const ConstraintSolverConf&) { auto& bodyConstraintA = At(bodies, GetBodyA()); auto& bodyConstraintB = At(bodies, GetBodyB()); diff --git a/PlayRho/Dynamics/Joints/WeldJoint.hpp b/PlayRho/Dynamics/Joints/WeldJoint.hpp index 17b635f2db..95491c352c 100644 --- a/PlayRho/Dynamics/Joints/WeldJoint.hpp +++ b/PlayRho/Dynamics/Joints/WeldJoint.hpp @@ -29,9 +29,11 @@ namespace playrho { /// @details You need to specify local anchor points /// where they are attached and the relative body angle. The position /// of the anchor points is important for computing the reaction torque. -struct WeldJointDef : public JointDef +struct WeldJointDef : public JointBuilder { - constexpr WeldJointDef() noexcept: JointDef(JointType::Weld) {} + using super = JointBuilder; + + constexpr WeldJointDef() noexcept: super{JointType::Weld} {} /// Initialize the bodies, anchors, and reference angle using a world /// anchor point. @@ -46,11 +48,13 @@ struct WeldJointDef : public JointDef /// The bodyB angle minus bodyA angle in the reference state (radians). Angle referenceAngle = Angle{0}; - /// The mass-spring-damper frequency in Hertz. Rotation only. - /// Disable softness with a value of 0. + /// @brief Mass-spring-damper frequency. + /// @note Rotation only. + /// @note Disable softness with a value of 0. Frequency frequency = Frequency{0}; - /// The damping ratio. 0 = no damping, 1 = critical damping. + /// @brief Damping ratio. + /// @note 0 = no damping, 1 = critical damping. Real dampingRatio = 0; }; @@ -77,8 +81,10 @@ class WeldJoint : public Joint /// Get the reference angle. Angle GetReferenceAngle() const { return m_referenceAngle; } - /// Set/get frequency. + /// @brief Sets frequency. void SetFrequency(Frequency frequency) { m_frequency = frequency; } + + /// @brief Gets the frequency. Frequency GetFrequency() const { return m_frequency; } /// Set/get damping ratio. @@ -88,23 +94,23 @@ class WeldJoint : public Joint private: void InitVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step, - const ConstraintSolverConf& conf) override; + const ConstraintSolverConf&) override; bool SolveVelocityConstraints(BodyConstraintsMap& bodies, const StepConf& step) override; bool SolvePositionConstraints(BodyConstraintsMap& bodies, const ConstraintSolverConf& conf) const override; + Length2D m_localAnchorA; + Length2D m_localAnchorB; + Angle m_referenceAngle; Frequency m_frequency; Real m_dampingRatio; - AngularVelocity m_bias; // Solver shared - Length2D m_localAnchorA; - Length2D m_localAnchorB; - Angle m_referenceAngle; - InvRotInertia m_gamma; Vec3 m_impulse = Vec3_zero; // Solver temp + InvRotInertia m_gamma; + AngularVelocity m_bias; Length2D m_rA; Length2D m_rB; Mat33 m_mass; diff --git a/PlayRho/Dynamics/Joints/WheelJoint.cpp b/PlayRho/Dynamics/Joints/WheelJoint.cpp index 5242305432..08f820fdf9 100644 --- a/PlayRho/Dynamics/Joints/WheelJoint.cpp +++ b/PlayRho/Dynamics/Joints/WheelJoint.cpp @@ -43,7 +43,7 @@ using namespace playrho; WheelJointDef::WheelJointDef(NonNull bA, NonNull bB, const Length2D anchor, const UnitVec2 axis) noexcept: - JointDef{JointType::Wheel, bA, bB}, + super{super{JointType::Wheel}.UseBodyA(bA).UseBodyB(bB)}, localAnchorA{GetLocalPoint(*bA, anchor)}, localAnchorB{GetLocalPoint(*bB, anchor)}, localAxisA{GetLocalVector(*bA, axis)} diff --git a/PlayRho/Dynamics/Joints/WheelJoint.hpp b/PlayRho/Dynamics/Joints/WheelJoint.hpp index d910ff6f13..bbdc9fd567 100644 --- a/PlayRho/Dynamics/Joints/WheelJoint.hpp +++ b/PlayRho/Dynamics/Joints/WheelJoint.hpp @@ -32,9 +32,11 @@ namespace playrho { /// can violate the constraint slightly. The joint translation is zero /// when the local anchor points coincide in world space. Using local /// anchors and a local axis helps when saving and loading a game. -struct WheelJointDef : public JointDef +struct WheelJointDef : public JointBuilder { - constexpr WheelJointDef() noexcept: JointDef(JointType::Wheel) {} + using super = JointBuilder; + + constexpr WheelJointDef() noexcept: super{JointType::Wheel} {} /// Initialize the bodies, anchors, axis, and reference angle using the world /// anchor and world axis. diff --git a/Testbed/Tests/Dominos.hpp b/Testbed/Tests/Dominos.hpp index f365cc08cb..7a02cdb3f1 100644 --- a/Testbed/Tests/Dominos.hpp +++ b/Testbed/Tests/Dominos.hpp @@ -77,10 +77,12 @@ class Dominos : public Test b3 = m_world->CreateBody(bd); auto conf = PolygonShape::Conf{}; conf.density = Real{10} * KilogramPerSquareMeter; - b3->CreateFixture(std::make_shared(Real{6.0f} * Meter, Real{0.125f} * Meter, conf)); + b3->CreateFixture(std::make_shared(Real{6.0f} * Meter, + Real{0.125f} * Meter, conf)); } - m_world->CreateJoint(RevoluteJointDef{b1, b3, Vec2(-2.0f, 1.0f) * Meter, true}); + m_world->CreateJoint(RevoluteJointDef{b1, b3, Vec2(-2.0f, 1.0f) * Meter} + .UseCollideConnected(true)); Body* b4; { @@ -90,10 +92,12 @@ class Dominos : public Test b4 = m_world->CreateBody(bd); auto conf = PolygonShape::Conf{}; conf.density = Real{10} * KilogramPerSquareMeter; - b4->CreateFixture(std::make_shared(Real{0.25f} * Meter, Real{0.25f} * Meter, conf)); + b4->CreateFixture(std::make_shared(Real{0.25f} * Meter, + Real{0.25f} * Meter, conf)); } - m_world->CreateJoint(RevoluteJointDef{b2, b4, Vec2(-7.0f, 15.0f) * Meter, true}); + m_world->CreateJoint(RevoluteJointDef{b2, b4, Vec2(-7.0f, 15.0f) * Meter} + .UseCollideConnected(true)); Body* b5; { @@ -108,17 +112,21 @@ class Dominos : public Test PolygonShape shape{conf}; - SetAsBox(shape, Real{1.0f} * Meter, Real{0.1f} * Meter, Vec2(0.0f, -0.9f) * Meter, Real{0.0f} * Radian); + SetAsBox(shape, Real{1.0f} * Meter, Real{0.1f} * Meter, Vec2(0.0f, -0.9f) * Meter, + Real{0.0f} * Radian); b5->CreateFixture(std::make_shared(shape)); - SetAsBox(shape, Real{0.1f} * Meter, Real{1.0f} * Meter, Vec2(-0.9f, 0.0f) * Meter, Real{0.0f} * Radian); + SetAsBox(shape, Real{0.1f} * Meter, Real{1.0f} * Meter, Vec2(-0.9f, 0.0f) * Meter, + Real{0.0f} * Radian); b5->CreateFixture(std::make_shared(shape)); - SetAsBox(shape, Real{0.1f} * Meter, Real{1.0f} * Meter, Vec2(0.9f, 0.0f) * Meter, Real{0.0f} * Radian); + SetAsBox(shape, Real{0.1f} * Meter, Real{1.0f} * Meter, Vec2(0.9f, 0.0f) * Meter, + Real{0.0f} * Radian); b5->CreateFixture(std::make_shared(shape)); } - m_world->CreateJoint(RevoluteJointDef{b1, b5, Vec2(6.0f, 2.0f) * Meter, true}); + m_world->CreateJoint(RevoluteJointDef{b1, b5, Vec2(6.0f, 2.0f) * Meter} + .UseCollideConnected(true)); Body* b6; { @@ -128,10 +136,12 @@ class Dominos : public Test b6 = m_world->CreateBody(bd); auto conf = PolygonShape::Conf{}; conf.density = Real{30} * KilogramPerSquareMeter; - b6->CreateFixture(std::make_shared(PolygonShape(Real{1.0f} * Meter, Real{0.1f} * Meter, conf))); + b6->CreateFixture(std::make_shared(PolygonShape(Real{1.0f} * Meter, + Real{0.1f} * Meter, conf))); } - m_world->CreateJoint(RevoluteJointDef{b5, b6, Vec2(7.5f, 4.0f) * Meter, true}); + m_world->CreateJoint(RevoluteJointDef{b5, b6, Vec2(7.5f, 4.0f) * Meter} + .UseCollideConnected(true)); Body* b7; { @@ -142,7 +152,8 @@ class Dominos : public Test b7 = m_world->CreateBody(bd); auto conf = PolygonShape::Conf{}; conf.density = Real{10} * KilogramPerSquareMeter; - b7->CreateFixture(std::make_shared(PolygonShape(Real{0.1f} * Meter, Real{1.0f} * Meter, conf))); + b7->CreateFixture(std::make_shared(PolygonShape(Real{0.1f} * Meter, + Real{1.0f} * Meter, conf))); } DistanceJointDef djd; @@ -164,7 +175,10 @@ class Dominos : public Test { BodyDef bd; bd.type = BodyType::Dynamic; - bd.position = Length2D(Real{5.9f} * Meter + Real{2.0f} * radius * static_cast(i), Real{2.4f} * Meter); + bd.position = Length2D{ + Real{5.9f} * Meter + Real{2.0f} * radius * static_cast(i), + Real{2.4f} * Meter + }; const auto body = m_world->CreateBody(bd); body->CreateFixture(shape); } diff --git a/Testbed/Tests/Pulleys.hpp b/Testbed/Tests/Pulleys.hpp index 3b32d7bc90..ba4bbd87ae 100644 --- a/Testbed/Tests/Pulleys.hpp +++ b/Testbed/Tests/Pulleys.hpp @@ -67,9 +67,9 @@ class Pulleys : public Test const auto groundAnchor1 = Vec2(-10.0f, y + b + L) * Meter; const auto groundAnchor2 = Vec2(10.0f, y + b + L) * Meter; const auto pulleyDef = PulleyJointDef{body1, body2, - groundAnchor1, groundAnchor2, anchor1, anchor2, 1.5f}; + groundAnchor1, groundAnchor2, anchor1, anchor2}.UseRatio(1.5f); - m_joint1 = (PulleyJoint*)m_world->CreateJoint(pulleyDef); + m_joint1 = static_cast(m_world->CreateJoint(pulleyDef)); } } diff --git a/Testbed/Tests/TheoJansen.hpp b/Testbed/Tests/TheoJansen.hpp index 3dd46702e4..9d612a75c4 100644 --- a/Testbed/Tests/TheoJansen.hpp +++ b/Testbed/Tests/TheoJansen.hpp @@ -76,18 +76,15 @@ class TheoJansen : public Test // Using a soft distance constraint can reduce some jitter. // It also makes the structure seem a bit more fluid by // acting like a suspension system. - m_world->CreateJoint(DistanceJointDef{ - body1, body2, p2 + m_offset, p5 + m_offset, Real(10) * Hertz, Real(0.5) - }); - m_world->CreateJoint(DistanceJointDef{ - body1, body2, p3 + m_offset, p4 + m_offset, Real(10) * Hertz, Real(0.5) - }); - m_world->CreateJoint(DistanceJointDef{ - body1, m_wheel, p3 + m_offset, wheelAnchor + m_offset, Real(10) * Hertz, Real(0.5) - }); - m_world->CreateJoint(DistanceJointDef{ - body2, m_wheel, p6 + m_offset, wheelAnchor + m_offset, Real(10) * Hertz, Real(0.5) - }); + + m_world->CreateJoint(DistanceJointDef{body1, body2, p2 + m_offset, p5 + m_offset} + .UseFrequency(Real(10) * Hertz).UseDampingRatio(Real(0.5))); + m_world->CreateJoint(DistanceJointDef{body1, body2, p3 + m_offset, p4 + m_offset} + .UseFrequency(Real(10) * Hertz).UseDampingRatio(Real(0.5))); + m_world->CreateJoint(DistanceJointDef{body1, m_wheel, p3 + m_offset, wheelAnchor + m_offset} + .UseFrequency(Real(10) * Hertz).UseDampingRatio(Real(0.5))); + m_world->CreateJoint(DistanceJointDef{body2, m_wheel, p6 + m_offset, wheelAnchor + m_offset} + .UseFrequency(Real(10) * Hertz).UseDampingRatio(Real(0.5))); m_world->CreateJoint(RevoluteJointDef{body2, m_chassis, p4 + m_offset}); } diff --git a/UnitTests/GearJoint.cpp b/UnitTests/GearJoint.cpp index 65b2bb0470..88a1fb63c0 100644 --- a/UnitTests/GearJoint.cpp +++ b/UnitTests/GearJoint.cpp @@ -56,8 +56,8 @@ TEST(GearJointDef, Traits) EXPECT_TRUE(std::is_nothrow_copy_constructible::value); EXPECT_TRUE(std::is_trivially_copy_constructible::value); - EXPECT_FALSE(std::is_copy_assignable::value); - EXPECT_FALSE(std::is_nothrow_copy_assignable::value); + EXPECT_TRUE(std::is_copy_assignable::value); + EXPECT_TRUE(std::is_nothrow_copy_assignable::value); EXPECT_FALSE(std::is_trivially_copy_assignable::value); EXPECT_TRUE(std::is_destructible::value); diff --git a/UnitTests/World.cpp b/UnitTests/World.cpp index 5a06651d19..be82b6b95e 100644 --- a/UnitTests/World.cpp +++ b/UnitTests/World.cpp @@ -244,7 +244,7 @@ TEST(World, CopyConstruction) world.CreateJoint(RevoluteJointDef{b1, b2, Length2D(0, 0)}); world.CreateJoint(PrismaticJointDef{b1, b2, Length2D(0, 0), UnitVec2::GetRight()}); world.CreateJoint(PulleyJointDef{b1, b2, Length2D(0, 0), Length2D(0, 0), - Length2D(0, 0), Length2D(0, 0), Real(1)}); + Length2D(0, 0), Length2D(0, 0)}.UseRatio(Real(1))); auto stepConf = StepConf{}; world.Step(stepConf); @@ -302,7 +302,7 @@ TEST(World, CopyAssignment) world.CreateJoint(RevoluteJointDef{b1, b2, Length2D(0, 0)}); world.CreateJoint(PrismaticJointDef{b1, b2, Length2D(0, 0), UnitVec2::GetRight()}); world.CreateJoint(PulleyJointDef{b1, b2, Length2D(0, 0), Length2D(0, 0), - Length2D(0, 0), Length2D(0, 0), Real(1)}); + Length2D(0, 0), Length2D(0, 0)}.UseRatio(Real(1))); auto stepConf = StepConf{}; world.Step(stepConf);