From b4651bbe0e684c8dab7669686104297ee7240aa1 Mon Sep 17 00:00:00 2001 From: Matthew Reid Date: Sat, 11 Jun 2022 16:06:46 +1000 Subject: [PATCH] SimpleDynamicBodyComponent available as fallback when not using bullet plugin --- .../SkyboltEngine/ComponentFactory.cpp | 10 +- .../Components/DummyDynamicBodyComponent.h | 42 -------- .../Components/DynamicBodyComponent.h | 1 + .../Components/SimpleDynamicBodyComponent.cpp | 81 ++++++++++++++ .../Components/SimpleDynamicBodyComponent.h | 63 +++++++++++ .../SimpleDynamicBodyComponentTest.cpp | 101 ++++++++++++++++++ 6 files changed, 254 insertions(+), 44 deletions(-) delete mode 100644 src/Skybolt/SkyboltSim/Components/DummyDynamicBodyComponent.h create mode 100644 src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.cpp create mode 100644 src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.h create mode 100644 src/Skybolt/SkyboltSimTests/SimpleDynamicBodyComponentTest.cpp diff --git a/src/Skybolt/SkyboltEngine/ComponentFactory.cpp b/src/Skybolt/SkyboltEngine/ComponentFactory.cpp index 13af7e9..852e8de 100644 --- a/src/Skybolt/SkyboltEngine/ComponentFactory.cpp +++ b/src/Skybolt/SkyboltEngine/ComponentFactory.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include @@ -193,7 +193,13 @@ static sim::ComponentPtr loadNode(Entity* entity, const ComponentFactoryContext& static sim::ComponentPtr loadDynamicBody(Entity* entity, const ComponentFactoryContext& context, const nlohmann::json& json) { - return std::make_shared(); + Node* node = entity->getFirstComponentRequired().get(); + Real mass = json.at("mass"); + Vector3 momentOfInertia = readOptionalVector3(json, "momentOfInertia"); + + auto component = std::make_shared(node, mass, momentOfInertia); + component->setCenterOfMass(readOptionalVector3(json, "centerOfMass")); + return component; } static sim::ComponentPtr loadAttachment(Entity* entity, const ComponentFactoryContext& context, const nlohmann::json& json) diff --git a/src/Skybolt/SkyboltSim/Components/DummyDynamicBodyComponent.h b/src/Skybolt/SkyboltSim/Components/DummyDynamicBodyComponent.h deleted file mode 100644 index a681054..0000000 --- a/src/Skybolt/SkyboltSim/Components/DummyDynamicBodyComponent.h +++ /dev/null @@ -1,42 +0,0 @@ -/* Copyright 2012-2020 Matthew Reid - * - * This Source Code Form is subject to the terms of the Mozilla Public - * License, v. 2.0. If a copy of the MPL was not distributed with this - * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ - - -#include "DynamicBodyComponent.h" -#include - -namespace skybolt { -namespace sim { - -class DummyDynamicBodyComponent : public DynamicBodyComponent -{ -public: - void setLinearVelocity(const Vector3& v) override {} - Vector3 getLinearVelocity() const override { return math::dvec3Zero(); } - - //! Angular velocity is in world space - void setAngularVelocity(const Vector3& v) {} - Vector3 getAngularVelocity() const override { return math::dvec3Zero(); } - - Real getMass() const override { return 0; } - - void setMass(Real mass) override {} - void setCenterOfMass(const Vector3& relPosition) override {} - - //! Apply force at center of mass. Force is in world axes. - void applyCentralForce(const Vector3& force) override {} - - //! Force and relPosition are in world axes - void applyForce(const Vector3& force, const Vector3& relPosition) override {} - - //! Apply torque. Torque is in world axes. - void applyTorque(const Vector3& torque) override {} - - virtual void setCollisionsEnabled(bool enabled) override {} -}; - -} // namespace sim -} // namespace skybolt \ No newline at end of file diff --git a/src/Skybolt/SkyboltSim/Components/DynamicBodyComponent.h b/src/Skybolt/SkyboltSim/Components/DynamicBodyComponent.h index a2d4e7a..061dcbf 100644 --- a/src/Skybolt/SkyboltSim/Components/DynamicBodyComponent.h +++ b/src/Skybolt/SkyboltSim/Components/DynamicBodyComponent.h @@ -40,6 +40,7 @@ class DynamicBodyComponent : public Component virtual void setMass(Real mass) = 0; virtual Real getMass() const = 0; + //! relPosition is in body axes virtual void setCenterOfMass(const Vector3& relPosition) = 0; //! Apply force at center of mass. Force is in world axes. diff --git a/src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.cpp b/src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.cpp new file mode 100644 index 0000000..1dc48d5 --- /dev/null +++ b/src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.cpp @@ -0,0 +1,81 @@ +/* Copyright 2012-2020 Matthew Reid + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + + +#include "SimpleDynamicBodyComponent.h" +#include "Node.h" +#include + +namespace skybolt { +namespace sim { + +SimpleDynamicBodyComponent::SimpleDynamicBodyComponent(Node* node, Real mass, const Vector3& momentofInertia) : + mNode(node), + mMass(mass), + mMomentOfInertia(momentofInertia) +{ +} + +void SimpleDynamicBodyComponent::applyCentralForce(const Vector3& force) +{ + mTotalForce += force; +} + +void SimpleDynamicBodyComponent::applyForce(const Vector3& force, const Vector3& relPosition) +{ + mTotalForce += force; + + Vector3 offset = relPosition - mNode->getOrientation() * mCenterOfMass; + mTotalTorque += cross(offset, force); +} + +void SimpleDynamicBodyComponent::applyTorque(const Vector3& torque) +{ + mTotalTorque += torque; +} + +void SimpleDynamicBodyComponent::updatePreDynamicsSubstep(TimeReal dtSubstep) +{ + double dt = (double)dtSubstep; + double halfDt2 = dt * dt * 0.5; + // integrate using velocity-verlet https://en.wikipedia.org/wiki/Verlet_integration + if (mMass > 0) + { + Vector3 acceleration = mTotalForce / (double)mMass; + mNode->setPosition(mNode->getPosition() + mLinearVelocity * dt + acceleration * halfDt2); + mLinearVelocity += acceleration * dt; + } + + if (mMomentOfInertia != math::dvec3Zero()) + { + Quaternion ori = mNode->getOrientation(); + Vector3 torqueBodySpace = inverse(ori) * mTotalTorque; + Vector3 angularAcceleration = ori * (torqueBodySpace / mMomentOfInertia); + + // https://gafferongames.com/post/physics_in_3d/ + Vector3 angularDisplacement = mAngularVelocity * dt + angularAcceleration * halfDt2; + Quaternion angularDisplacementQuat( + 0, + angularDisplacement.x, + angularDisplacement.y, + angularDisplacement.z); + + ori += 0.5 * angularDisplacementQuat * ori; + ori = normalize(ori); + mNode->setOrientation(ori); + + mAngularVelocity += angularAcceleration * dt; + } +} + +void SimpleDynamicBodyComponent::updatePostDynamics(TimeReal dt, TimeReal dtWallClock) +{ + mTotalForce = math::dvec3Zero(); + mTotalTorque = math::dvec3Zero(); +} + +} // namespace sim +} // namespace skybolt \ No newline at end of file diff --git a/src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.h b/src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.h new file mode 100644 index 0000000..034f73e --- /dev/null +++ b/src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.h @@ -0,0 +1,63 @@ +/* Copyright 2012-2020 Matthew Reid + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + + +#include "DynamicBodyComponent.h" +#include + +namespace skybolt { +namespace sim { + +class SimpleDynamicBodyComponent : public DynamicBodyComponent +{ +public: + SimpleDynamicBodyComponent(Node* node, Real mass, const Vector3& momentofInertia); + void setLinearVelocity(const Vector3& v) override { mLinearVelocity = v; } + Vector3 getLinearVelocity() const override { return mLinearVelocity; } + + //! Angular velocity is in world space + void setAngularVelocity(const Vector3& v) override { mAngularVelocity = v; } + Vector3 getAngularVelocity() const override { return mAngularVelocity; } + + Real getMass() const override { return mMass; } + + void setMass(Real mass) override { mMass = mass; } + void setCenterOfMass(const Vector3& relPosition) override { mCenterOfMass = relPosition; } + + //! Apply force at center of mass. Force is in world axes. + void applyCentralForce(const Vector3& force) override; + + //! Force and relPosition are in world axes + void applyForce(const Vector3& force, const Vector3& relPosition) override; + + //! Apply torque. Torque is in world axes. + void applyTorque(const Vector3& torque) override; + + void updatePreDynamicsSubstep(TimeReal dtSubstep) override; + void updatePostDynamics(TimeReal dt, TimeReal dtWallClock) override; + + virtual void setCollisionsEnabled(bool enabled) override {} + + std::vector getExposedTypes() const override + { + return {typeid(DynamicBodyComponent), typeid(SimpleDynamicBodyComponent)}; + } + +private: + Node* mNode; + Real mMass; + Vector3 mMomentOfInertia; + Vector3 mCenterOfMass = math::dvec3Zero(); + + Vector3 mLinearVelocity = math::dvec3Zero(); //!< World space + Vector3 mAngularVelocity = math::dvec3Zero(); //!< World space + + Vector3 mTotalForce = math::dvec3Zero(); //!< World space + Vector3 mTotalTorque = math::dvec3Zero(); //!< World space +}; + +} // namespace sim +} // namespace skybolt \ No newline at end of file diff --git a/src/Skybolt/SkyboltSimTests/SimpleDynamicBodyComponentTest.cpp b/src/Skybolt/SkyboltSimTests/SimpleDynamicBodyComponentTest.cpp new file mode 100644 index 0000000..f9d0165 --- /dev/null +++ b/src/Skybolt/SkyboltSimTests/SimpleDynamicBodyComponentTest.cpp @@ -0,0 +1,101 @@ +/* Copyright 2012-2020 Matthew Reid + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ + + +#include +#include +#include +#include + +#include + +using namespace skybolt; +using namespace skybolt::sim; + +static void integrateOverTime(SimpleDynamicBodyComponent& body, TimeReal duration) +{ + constexpr TimeReal dt = 0.01f; + for (TimeReal t = 0; t < duration; t += dt) + { + body.updatePreDynamicsSubstep(dt); + } +} + +TEST_CASE("Body accelerates under force") +{ + Node node; + Real mass = 5; + Vector3 momentOfInertia(2, 3, 4); + SimpleDynamicBodyComponent body(&node, mass, momentOfInertia); + + // Calculate expected results + // https://www.calculatorsoup.com/calculators/physics/displacement_v_a_t.php + float t = 3; + float f = 10; + Real a = f / mass; + Real s = 0.5f * a * t * t; + + // Simulate + body.applyCentralForce(Vector3(f, 0, 0)); + integrateOverTime(body, t); + + CHECK(node.getPosition().x == Approx(s).epsilon(0.01)); + CHECK(node.getPosition().y == Approx(0).epsilon(1e-8f)); + CHECK(node.getPosition().z == Approx(0).epsilon(1e-8f)); +} + +TEST_CASE("Body rotates under torque") +{ + Node node; + Real mass = 5; + Vector3 momentOfInertia(2, 3, 4); + SimpleDynamicBodyComponent body(&node, mass, momentOfInertia); + + // Calculate expected results + // https://www.calculatorsoup.com/calculators/physics/displacement_v_a_t.php + float t = 3; + float T = 0.1f; + Real a = T / Real(momentOfInertia.x); + Real theta = 0.5f * a * t * t; + + // Simulate + body.applyTorque(Vector3(T, 0, 0)); + integrateOverTime(body, t); + + Vector3 euler = math::eulerFromQuat(node.getOrientation()); + + CHECK(euler.x == Approx(theta).epsilon(0.01)); + CHECK(euler.y == Approx(0).epsilon(1e-8f)); + CHECK(euler.z == Approx(0).epsilon(1e-8f)); +} + +TEST_CASE("Force at distance produces torque") +{ + Node node; + Real mass = 5; + Vector3 momentOfInertia(2, 3, 4); + SimpleDynamicBodyComponent body(&node, mass, momentOfInertia); + Vector3 centerOfMass(0.2, 0.3, 0.4); + body.setCenterOfMass(centerOfMass); + + // Calculate expected results + // https://www.calculatorsoup.com/calculators/physics/displacement_v_a_t.php + float t = 3; + float f = 0.1f; + float offset = 0.5f; + Real a = f * offset / Real(momentOfInertia.x); + Real theta = 0.5f * a * t * t; + + // Simulate + body.applyForce(Vector3(0, 0, f), Vector3(0, offset, 0) + centerOfMass); + integrateOverTime(body, t); + + Vector3 euler = math::eulerFromQuat(node.getOrientation()); + + CHECK(euler.x == Approx(theta).epsilon(0.01)); + CHECK(euler.y == Approx(0).epsilon(1e-8f)); + CHECK(euler.z == Approx(0).epsilon(1e-8f)); +} \ No newline at end of file