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

app/src/Wheel.cpp
107 строк3 KB
wmigor
Расчет аэродинмамических сил перенесе из AircraftPhysics в Wing, Thruster и Fuselage
10 июн 2026, 15:12
10 июн 2026, 15:12534021f
#include "Wheel.h"
#include "asim/Constants.h"
#include <glm/ext.hpp>

Wheel::Wheel(const asim::Input *input, const Config& config): _input(input), _config(config)
{
}

bool Wheel::calculateForce(const RigidBody *body, double delta, glm::dvec3 &outForce, glm::dvec3 &outTorque)
{
	outForce = glm::dvec3(0.0);
	outTorque = glm::dvec3(0.0);
	setSteer(-_input->rudder);
	if (!calculateSpring(body, delta, outForce, outTorque))
		return false;

	glm::dvec3 tireForce, tireTorque;
	if (calculateTire(body, tireForce, tireTorque))
	{
		outForce += tireForce;
		outTorque += tireTorque;
		return true;
	}

	return false;
}

bool Wheel::calculateSpring(const RigidBody *body, double delta, glm::dvec3 &outForce, glm::dvec3 &outTorque)
{
	auto point = body->position + body->rotation * _config.position;
	point.y -= _config.radius;

	if (point.y > 0.0)
	{
		_compress = glm::mix(_compress, 0.0, delta);
		return false;
	}

	auto normal = asim::Constants::Up;

	_compress = -point.y;
	auto radius = point - body->getMassCenterGlobal();
	auto contactVelocity = body->linearVelocity + glm::cross(body->angularVelocity, radius);
	auto compressVelocityMax = _compress / delta;
	auto compressVelocity = contactVelocity.y;

	if (glm::abs(compressVelocity) > compressVelocityMax)
		compressVelocity = glm::sign(compressVelocity) * compressVelocityMax;

	outForce = normal * (_compress * _config.stiffness - compressVelocity * _config.damping);
	outTorque = glm::cross(radius, outForce);
	_springForce = glm::dot(outForce, normal);
	return true;
}

bool Wheel::calculateTire(const RigidBody *body, glm::dvec3 &outForce, glm::dvec3 &outTorque)
{
	if (_springForce <= 0.0)
		return false;

	auto point = body->position + body->rotation * _config.position;
	point.y -= _config.radius;

	if (point.y > 0.0)
		return false;

	auto normal = asim::Constants::Up;
	auto forward = body->rotation * asim::Constants::Forward;
	auto right = glm::cross(forward, normal);

	if (_steerAngle != 0.0)
		right = glm::rotate(right, _steerAngle, normal);

	auto radius = point - body->getMassCenterGlobal();
	auto pointVelocity = body->linearVelocity + glm::cross(body->angularVelocity, radius);
	auto slideVelocityScalar = glm::dot(pointVelocity, right);
	auto forwardVelocityScalar = glm::dot(pointVelocity, forward);
	auto slipAngle = -glm::atan2(slideVelocityScalar, -forwardVelocityScalar);
	auto wheelForce = _springForce * calculateSlideFrictionFactor(slipAngle);

	auto force = wheelForce * right;

	if (_input->brake > 0.0)
	{
		auto brakeForce = -glm::sign(forwardVelocityScalar) * _springForce * _config.brakeFriction * _input->brake;
		force += brakeForce * forward;
	}

	outForce = _lastTireForce + (force - _lastTireForce) * 0.5;
	_lastTireForce = outForce;
	outTorque = glm::cross(radius, outForce);

	return true;
}


double Wheel::calculateSlideFrictionFactor(double slipAngle)
{
	auto peak = 0.8;
	auto stiffness = 0.06;
	auto shape = 2.8;
	auto curvature = 1.03;
	auto stiffSlip = stiffness * slipAngle;
	auto factor = peak * glm::sin(shape * glm::atan(stiffSlip - curvature * (stiffSlip - glm::atan(stiffSlip))));

	return factor;
}