SimpleDynamicBodyComponent available as fallback when not using bulle…

…t plugin

src/Skybolt/SkyboltEngine/ComponentFactory.cpp

@@ -19,7 +19,7 @@
 #include <SkyboltSim/Components/CameraComponent.h>
 #include <SkyboltSim/Components/CameraControllerComponent.h>
 #include <SkyboltSim/Components/ControlInputsComponent.h>
-#include <SkyboltSim/Components/DummyDynamicBodyComponent.h>
+#include <SkyboltSim/Components/SimpleDynamicBodyComponent.h>
 #include <SkyboltSim/Components/FuselageComponent.h>
 #include <SkyboltSim/Components/JetTurbineComponent.h>
 #include <SkyboltSim/Components/MainRotorComponent.h>
@@ -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<DummyDynamicBodyComponent>();
+	Node* node = entity->getFirstComponentRequired<Node>().get();
+	Real mass = json.at("mass");
+	Vector3 momentOfInertia = readOptionalVector3(json, "momentOfInertia");
+
+	auto component = std::make_shared<SimpleDynamicBodyComponent>(node, mass, momentOfInertia);
+	component->setCenterOfMass(readOptionalVector3(json, "centerOfMass"));
+	return component;
 }
 
 static sim::ComponentPtr loadAttachment(Entity* entity, const ComponentFactoryContext& context, const nlohmann::json& json)

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.

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 <SkyboltCommon/Math/MathUtility.h>
+
+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

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 <SkyboltCommon/Math/MathUtility.h>
+
+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<std::type_index> 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

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 <SkyboltSim/Components/SimpleDynamicBodyComponent.h>
+#include <SkyboltSim/Components/Node.h>
+#include <SkyboltSim/SimMath.h>
+#include <catch2/catch.hpp>
+
+#include <assert.h>
+
+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));
+}