app/src/RigidBody.cpp - master - wmigor/z-plane

app/src/RigidBody.cpp
168 строк4 KB
wmigor
Добавлена простая физика фюзеляжа
06 июн 2026, 00:15
06 июн 2026, 00:15699fc8c
#include "RigidBody.h"
#include <glm/gtx/quaternion.hpp>
#include <asim/tools.h>

RigidBody::RigidBody()
{
	setInertia(glm::dvec3(1.0));
}

RigidBody::~RigidBody()
{
	for (auto force : _forces)
		delete force;
}

void RigidBody::update(double delta)
{
	applyForces(delta);
	integrateVelocity(delta);
	applyImpulses(delta);
	integratePosition(delta);
	pushFromGround();
}

void RigidBody::integrateVelocity(double delta)
{
	auto acceleration = _force * _massInverted + gravity;
	auto angularAcceleration = _inertiaTensorInverted * _torque;

	linearVelocity += acceleration * delta;
	angularVelocity += angularAcceleration * delta;

	_force = glm::vec3(0.0);
	_torque = glm::vec3(0.0);
}

void RigidBody::integratePosition(double delta)
{
	position += linearVelocity * delta;
	rotation = glm::dquat(angularVelocity * delta) * rotation;
	updateInertia();
}

void RigidBody::updateInertia()
{
	auto orientation = glm::toMat3(rotation);
	auto orientationTransposed = glm::transpose(orientation);
	glm::dmat3 inertiaTensor;
	inertiaTensor[0] = glm::dvec3(1.0 / _inertia.x, 0.0, 0.0);
	inertiaTensor[1] = glm::dvec3(0.0, 1.0 / _inertia.y, 0.0);
	inertiaTensor[2] = glm::dvec3(0.0, 0.0, 1.0 / _inertia.z);
	_inertiaTensorInverted = orientation * inertiaTensor * orientationTransposed;
	_massCenterGlobal = rotation * _massCenter + position;
}

void RigidBody::applyForce(const glm::dvec3& force)
{
	_force += force;
}

void RigidBody::applyForce(const glm::dvec3& force, const glm::dvec3& point)
{
	auto radius = point - _massCenterGlobal;
	_force += force;
	_torque += glm::cross(radius, force);
}


void RigidBody::applyTorque(const glm::dvec3& torque)
{
	_torque += torque;
}

void RigidBody::applyImpulse(const glm::dvec3& impulse, const glm::dvec3& point)
{
	auto radius = point - _massCenterGlobal;

	angularVelocity += _inertiaTensorInverted * glm::cross(radius, impulse);
	linearVelocity += impulse / _mass;
}

void RigidBody::setMass(double mass)
{
	_mass = mass;
	_massInverted = mass != 0.0 ? 1.0 / mass : 0.0;
}

void RigidBody::setMassCenter(const glm::dvec3& massCenter)
{
	_massCenter = massCenter;
	updateInertia();
}


void RigidBody::setInertia(const glm::dvec3& inertia)
{
	_inertia = inertia;
	updateInertia();
}

void RigidBody::addContactPoint(const glm::dvec3& position, double dynamicFriction)
{
	ContactPoint point;
	point.position = position;
	point.dynamicFriction = dynamicFriction;
	_contactPoints.push_back(point);
}


void RigidBody::applyForces(double delta)
{
	glm::dvec3 force, torque;

	for (auto f : _forces)
	{
		if (f->calculateForce(this, delta, force, torque))
		{
			applyForce(force);
			applyTorque(torque);
		}
	}
}

void RigidBody::applyImpulses(double delta)
{
	auto normal = glm::dvec3(0.0, 1.0, 0.0);

	for (const auto &contact : _contactPoints)
	{
		auto point = position + rotation * contact.position;
		if (point.y > 0.0)
			continue;

		auto radius = point - _massCenterGlobal;
		auto contactVelocity = linearVelocity + glm::cross(angularVelocity, radius);

		auto normalVelocity = glm::dot(contactVelocity, normal);
		auto normalInertiaInverted = _inertiaTensorInverted * glm::cross(radius, normal);
		auto normalMass = 1.0 / (_massInverted + glm::dot(normal, glm::cross(normalInertiaInverted, radius)));
		auto normalImpulse = glm::min(0.0, normalVelocity * normalMass);

		auto slideVelocity = contactVelocity - normal * normalVelocity;
		auto tangent = asim::safeNormalize(slideVelocity);
		auto tangentVelocity = glm::dot(slideVelocity, tangent);
		auto tangentInertiaInverted = _inertiaTensorInverted * glm::cross(radius, tangent);
		auto tangentMass = 1.0 / (_massInverted + glm::dot(tangent, glm::cross(tangentInertiaInverted, radius)));
		auto tangentImpulse = tangentMass * tangentVelocity;
		auto friction = contact.dynamicFriction;

		auto impulse = normalImpulse * normal + tangentImpulse * friction * tangent;
		applyImpulse(-impulse, point);
	}
}

void RigidBody::pushFromGround()
{
	auto minY = 0.0;

	for (const auto &contact : _contactPoints)
	{
		auto point = position + rotation * contact.position;
		if (point.y < minY)
			minY = point.y;
	}

	if (minY < 0.0)
		position.y -= minY;
}