|
/*
|
|
* Copyright (C) 2023 Christopher J. Howard
|
|
*
|
|
* This file is part of Antkeeper source code.
|
|
*
|
|
* Antkeeper source code is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* Antkeeper source code is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with Antkeeper source code. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <engine/physics/kinematics/rigid-body.hpp>
|
|
#include <engine/math/interpolation.hpp>
|
|
#include <algorithm>
|
|
|
|
namespace physics {
|
|
|
|
void rigid_body::integrate_forces(float dt) noexcept
|
|
{
|
|
// Apply forces
|
|
m_linear_momentum += m_applied_force * dt;
|
|
m_angular_momentum += m_applied_torque * dt;
|
|
|
|
// Apply damping
|
|
m_linear_momentum *= std::max<float>(0.0f, 1.0f - m_linear_damping * dt);
|
|
m_angular_momentum *= std::max<float>(0.0f, 1.0f - m_angular_damping * dt);
|
|
|
|
// Update velocities
|
|
m_linear_velocity = m_inverse_mass * m_linear_momentum;
|
|
m_angular_velocity = m_inverse_inertia * m_angular_momentum;
|
|
|
|
// Clear forces
|
|
m_applied_force = math::fvec3::zero();
|
|
m_applied_torque = math::fvec3::zero();
|
|
}
|
|
|
|
void rigid_body::integrate_velocities(float dt) noexcept
|
|
{
|
|
// Record previous state
|
|
m_previous_transform = m_current_transform;
|
|
|
|
// Update position
|
|
m_current_transform.translation += m_linear_velocity * dt;
|
|
|
|
// Update orientation
|
|
const math::fquat spin = math::fquat{0.0f, m_angular_velocity * 0.5f} * m_current_transform.rotation;
|
|
m_current_transform.rotation = math::normalize(m_current_transform.rotation + spin * dt);
|
|
}
|
|
|
|
math::transform<float> rigid_body::interpolate(float alpha) const
|
|
{
|
|
return
|
|
{
|
|
math::lerp(m_previous_transform.translation, m_current_transform.translation, alpha),
|
|
math::nlerp(m_previous_transform.rotation, m_current_transform.rotation, alpha),
|
|
math::lerp(m_previous_transform.scale, m_current_transform.scale, alpha),
|
|
};
|
|
}
|
|
|
|
} // namespace physics
|