spring-constraint.cpp

Go to the documentation of this file.

/*
 * 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/constraints/spring-constraint.hpp>
 
namespace physics {
 
void spring_constraint::solve(float dt)
{
    if (!m_body_a || !m_body_b)
    {
        return;
    }
    
    // Get radius vectors from centers of mass to spring attachment points
    const math::fvec3 radius_a = m_body_a->get_orientation() * m_point_a;
    const math::fvec3 radius_b = m_body_b->get_orientation() * m_point_b;
    
    // Get world-space spring attachment points
    const math::fvec3 point_a = m_body_a->get_position() + radius_a;
    const math::fvec3 point_b = m_body_b->get_position() + radius_b;
    
    // Calculate relative velocity between the attachment points
    const math::fvec3 velocity = m_body_b->get_point_velocity(radius_b) - m_body_a->get_point_velocity(radius_a);
    
    // Calculate spring force
    // F = -k * (|x| - d) * (x / |x|) - bv
    const math::fvec3 difference = point_b - point_a;
    const float distance = std::sqrt(math::dot(difference, difference));
    const math::fvec3 force = -m_stiffness * (distance - m_resting_length) * (difference / distance) - velocity * m_damping;
    
    // Apply spring force to bodies at attachment points
    m_body_a->apply_force(-force, radius_a);
    m_body_b->apply_force(force, radius_b);
}
 
} // namespace physics