diff options
Diffstat (limited to 'Client/ThirdParty/Box2D/src/dynamics/b2_joint.cpp')
| -rw-r--r-- | Client/ThirdParty/Box2D/src/dynamics/b2_joint.cpp | 301 |
1 files changed, 301 insertions, 0 deletions
diff --git a/Client/ThirdParty/Box2D/src/dynamics/b2_joint.cpp b/Client/ThirdParty/Box2D/src/dynamics/b2_joint.cpp new file mode 100644 index 0000000..41addbb --- /dev/null +++ b/Client/ThirdParty/Box2D/src/dynamics/b2_joint.cpp @@ -0,0 +1,301 @@ +// MIT License + +// Copyright (c) 2019 Erin Catto + +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: + +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. + +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "box2d/b2_block_allocator.h" +#include "box2d/b2_body.h" +#include "box2d/b2_distance_joint.h" +#include "box2d/b2_draw.h" +#include "box2d/b2_friction_joint.h" +#include "box2d/b2_gear_joint.h" +#include "box2d/b2_motor_joint.h" +#include "box2d/b2_mouse_joint.h" +#include "box2d/b2_prismatic_joint.h" +#include "box2d/b2_pulley_joint.h" +#include "box2d/b2_revolute_joint.h" +#include "box2d/b2_weld_joint.h" +#include "box2d/b2_wheel_joint.h" +#include "box2d/b2_world.h" + +#include <new> + +void b2LinearStiffness(float& stiffness, float& damping, + float frequencyHertz, float dampingRatio, + const b2Body* bodyA, const b2Body* bodyB) +{ + float massA = bodyA->GetMass(); + float massB = bodyB->GetMass(); + float mass; + if (massA > 0.0f && massB > 0.0f) + { + mass = massA * massB / (massA + massB); + } + else if (massA > 0.0f) + { + mass = massA; + } + else + { + mass = massB; + } + + float omega = 2.0f * b2_pi * frequencyHertz; + stiffness = mass * omega * omega; + damping = 2.0f * mass * dampingRatio * omega; +} + +void b2AngularStiffness(float& stiffness, float& damping, + float frequencyHertz, float dampingRatio, + const b2Body* bodyA, const b2Body* bodyB) +{ + float IA = bodyA->GetInertia(); + float IB = bodyB->GetInertia(); + float I; + if (IA > 0.0f && IB > 0.0f) + { + I = IA * IB / (IA + IB); + } + else if (IA > 0.0f) + { + I = IA; + } + else + { + I = IB; + } + + float omega = 2.0f * b2_pi * frequencyHertz; + stiffness = I * omega * omega; + damping = 2.0f * I * dampingRatio * omega; +} + +b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator) +{ + b2Joint* joint = nullptr; + + switch (def->type) + { + case e_distanceJoint: + { + void* mem = allocator->Allocate(sizeof(b2DistanceJoint)); + joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def)); + } + break; + + case e_mouseJoint: + { + void* mem = allocator->Allocate(sizeof(b2MouseJoint)); + joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def)); + } + break; + + case e_prismaticJoint: + { + void* mem = allocator->Allocate(sizeof(b2PrismaticJoint)); + joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def)); + } + break; + + case e_revoluteJoint: + { + void* mem = allocator->Allocate(sizeof(b2RevoluteJoint)); + joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def)); + } + break; + + case e_pulleyJoint: + { + void* mem = allocator->Allocate(sizeof(b2PulleyJoint)); + joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def)); + } + break; + + case e_gearJoint: + { + void* mem = allocator->Allocate(sizeof(b2GearJoint)); + joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def)); + } + break; + + case e_wheelJoint: + { + void* mem = allocator->Allocate(sizeof(b2WheelJoint)); + joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def)); + } + break; + + case e_weldJoint: + { + void* mem = allocator->Allocate(sizeof(b2WeldJoint)); + joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def)); + } + break; + + case e_frictionJoint: + { + void* mem = allocator->Allocate(sizeof(b2FrictionJoint)); + joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def)); + } + break; + + case e_motorJoint: + { + void* mem = allocator->Allocate(sizeof(b2MotorJoint)); + joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def)); + } + break; + + default: + b2Assert(false); + break; + } + + return joint; +} + +void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator) +{ + joint->~b2Joint(); + switch (joint->m_type) + { + case e_distanceJoint: + allocator->Free(joint, sizeof(b2DistanceJoint)); + break; + + case e_mouseJoint: + allocator->Free(joint, sizeof(b2MouseJoint)); + break; + + case e_prismaticJoint: + allocator->Free(joint, sizeof(b2PrismaticJoint)); + break; + + case e_revoluteJoint: + allocator->Free(joint, sizeof(b2RevoluteJoint)); + break; + + case e_pulleyJoint: + allocator->Free(joint, sizeof(b2PulleyJoint)); + break; + + case e_gearJoint: + allocator->Free(joint, sizeof(b2GearJoint)); + break; + + case e_wheelJoint: + allocator->Free(joint, sizeof(b2WheelJoint)); + break; + + case e_weldJoint: + allocator->Free(joint, sizeof(b2WeldJoint)); + break; + + case e_frictionJoint: + allocator->Free(joint, sizeof(b2FrictionJoint)); + break; + + case e_motorJoint: + allocator->Free(joint, sizeof(b2MotorJoint)); + break; + + default: + b2Assert(false); + break; + } +} + +b2Joint::b2Joint(const b2JointDef* def) +{ + b2Assert(def->bodyA != def->bodyB); + + m_type = def->type; + m_prev = nullptr; + m_next = nullptr; + m_bodyA = def->bodyA; + m_bodyB = def->bodyB; + m_index = 0; + m_collideConnected = def->collideConnected; + m_islandFlag = false; + m_userData = def->userData; + + m_edgeA.joint = nullptr; + m_edgeA.other = nullptr; + m_edgeA.prev = nullptr; + m_edgeA.next = nullptr; + + m_edgeB.joint = nullptr; + m_edgeB.other = nullptr; + m_edgeB.prev = nullptr; + m_edgeB.next = nullptr; +} + +bool b2Joint::IsEnabled() const +{ + return m_bodyA->IsEnabled() && m_bodyB->IsEnabled(); +} + +void b2Joint::Draw(b2Draw* draw) const +{ + const b2Transform& xf1 = m_bodyA->GetTransform(); + const b2Transform& xf2 = m_bodyB->GetTransform(); + b2Vec2 x1 = xf1.p; + b2Vec2 x2 = xf2.p; + b2Vec2 p1 = GetAnchorA(); + b2Vec2 p2 = GetAnchorB(); + + b2Color color(0.5f, 0.8f, 0.8f); + + switch (m_type) + { + case e_distanceJoint: + draw->DrawSegment(p1, p2, color); + break; + + case e_pulleyJoint: + { + b2PulleyJoint* pulley = (b2PulleyJoint*)this; + b2Vec2 s1 = pulley->GetGroundAnchorA(); + b2Vec2 s2 = pulley->GetGroundAnchorB(); + draw->DrawSegment(s1, p1, color); + draw->DrawSegment(s2, p2, color); + draw->DrawSegment(s1, s2, color); + } + break; + + case e_mouseJoint: + { + b2Color c; + c.Set(0.0f, 1.0f, 0.0f); + draw->DrawPoint(p1, 4.0f, c); + draw->DrawPoint(p2, 4.0f, c); + + c.Set(0.8f, 0.8f, 0.8f); + draw->DrawSegment(p1, p2, c); + + } + break; + + default: + draw->DrawSegment(x1, p1, color); + draw->DrawSegment(p1, p2, color); + draw->DrawSegment(x2, p2, color); + } +} |