-
Notifications
You must be signed in to change notification settings - Fork 43
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
SimpleDynamicBodyComponent available as fallback when not using bulle…
…t plugin
- Loading branch information
Matthew Reid
committed
Jun 11, 2022
1 parent
f475a60
commit b4651bb
Showing
6 changed files
with
254 additions
and
44 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
42 changes: 0 additions & 42 deletions
42
src/Skybolt/SkyboltSim/Components/DummyDynamicBodyComponent.h
This file was deleted.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
81 changes: 81 additions & 0 deletions
81
src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
63 changes: 63 additions & 0 deletions
63
src/Skybolt/SkyboltSim/Components/SimpleDynamicBodyComponent.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
101 changes: 101 additions & 0 deletions
101
src/Skybolt/SkyboltSimTests/SimpleDynamicBodyComponentTest.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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)); | ||
} |