CarJoints.cpp

# include <Siv3D.hpp>

Quad LineToQuad(const Vec2& p0, const Vec2& p1, double thickness = 10.0)
{
	const Vec2 center = ((p0 + p1) * 0.5);
	const double agnle = (p1 - p0).getAngle();
	return RectF{ Arg::center = center, thickness, (p1 - p0).length() }.rotatedAt(center, agnle);
}

struct Car
{
	// 円
	Array<P2Body> circles;

	// スティック
	Array<P2Body> sticks;

	// 円とスティックのジョイント
	Array<P2DistanceJoint> joints;

	// 車輪
	Array<P2Body> wheels;

	// 車輪とスティックのジョイント
	Array<P2WheelJoint> wheelJoints;

	void draw() const
	{
		for (const auto& stick : sticks)
		{
			stick.draw(HSV{ stick.id() * 10.0 });
		}

		for (const auto& circle : circles)
		{
			circle.draw();
		}

		for (const auto& wheel : wheels)
		{
			wheel.draw(ColorF{ 0.25 }).drawWireframe(2, Palette::Orange);
		}
	}

	Vec2 getPos() const
	{
		if (circles.isEmpty())
		{
			return Vec2{ 0, 0 };
		}

		return circles[0].getPos();
	}
};

Car CreateCar(P2World& world, const Vec2& pos,
	const Array<Vec2>& vertices, const Array<std::pair<int32, int32>>& indices,
	const Array<int32>& wheelStickIndices)
{
	Car car;

	for (const auto& vertex : vertices)
	{
		car.circles << world.createCircle(P2Dynamic, (pos + vertex), 10.0);
	}

	for (const auto& index : indices)
	{
		car.sticks << world.createQuad(P2Dynamic, pos, LineToQuad(vertices[index.first], vertices[index.second]));
	}

	constexpr double Damping = 1.0;
	constexpr double Stiffness = 100.0;
	constexpr double MaxLength = 5.0;

	for (size_t stickIndex = 0; const auto& index : indices)
	{
		const Vec2 stickLeft = (pos + vertices[index.first]);
		car.joints << world.createDistanceJoint(car.circles[index.first], stickLeft, car.sticks[stickIndex], stickLeft, 0.0)
			.setDamping(Damping).setStiffness(Stiffness).setMaxLength(MaxLength);

		const Vec2 stickRight = (pos + vertices[index.second]);
		car.joints << world.createDistanceJoint(car.circles[index.second], stickRight, car.sticks[stickIndex], stickRight, 0.0)
			.setDamping(Damping).setStiffness(Stiffness).setMaxLength(MaxLength);

		++stickIndex;
	}

	constexpr double DampingRatio = 0.5;

	for (size_t i = 0; const auto& wheelStickIndex : wheelStickIndices)
	{
		const Line stick{ (pos + vertices[indices[wheelStickIndex].first]), (pos + vertices[indices[wheelStickIndex].second]) };
		const Vec2 wheelPos = stick.center();

		car.wheels << world.createCircle(P2Dynamic, wheelPos, 30)
			.setAngularDamping(1.5); // 回転の減衰

		car.wheelJoints << world.createWheelJoint(car.sticks[wheelStickIndex], car.wheels[i], car.wheels[i].getPos(), Vec2{ 0, -1 })
			.setLinearStiffness(4.0, DampingRatio)
			.setLimits(-5, 5).setLimitsEnabled(true)
			.setMaxMotorTorque(1000).setMotorEnabled(true);

		++i;
	}

	return car;
}

void Main()
{
	Window::Resize(1280, 720);

	constexpr double StepTime = (1.0 / 200.0);
	double accumulatedTime = 0.0;

	P2World world;

	Array<P2Body> grounds;
	grounds << world.createRect(P2Static, Vec2{ 0, 0 }, SizeF{ 1000, 10 });

	Array<Car> cars;
	Array<P2Body> bodies;

	Camera2D camera{ Vec2{ 0, -300 }, 1.0 };

	// 頂点
	const Array<Vec2> vertices = {
		{ -200, 0 },
		{ -100, -100 },
		{ 0, 0 },
		{ 100, -100 },
		{ 200, 0 },
	};

	// 頂点のインデックスのペア（スティック）
	const Array<std::pair<int32, int32>> indices =
	{
		{0, 1}, {1, 2}, { 2, 0 }, { 1, 3 }, { 3, 2 }, { 3, 4 }, { 4, 2 }
	};

	while (System::Update())
	{
		for (accumulatedTime += Scene::DeltaTime(); StepTime <= accumulatedTime; accumulatedTime -= StepTime)
		{
			world.update(StepTime);

			cars.remove_if([](const Car& car) { return (500 < car.getPos().y); });

			bodies.remove_if([](const P2Body& body) { return (500 < body.getPos().y); });
		}

		camera.update();
		{
			const auto t = camera.createTransformer();

			for (const auto& ground : grounds)
			{
				ground.draw(Palette::Gray);
			}

			for (const auto& car : cars)
			{
				car.draw();
			}

			for (const auto& body : bodies)
			{
				body.draw(HSV{ body.id() * 10.0 });
			}
		}

		camera.draw(Palette::Orange);

		if (SimpleGUI::Button(U"Reset", Vec2{ 40, 40 }, 120))
		{
			cars.clear();
			bodies.clear();
		}

		if (SimpleGUI::Button(U"Rect", Vec2{ 40, 80 }, 120))
		{
			bodies << world.createRect(P2Dynamic, Vec2{ Random(-200, 200), -600 }, SizeF{ 40, 40 }, P2Material{ .density = 0.1 });
		}

		if (SimpleGUI::Button(U"Car", Vec2{ 40, 120 }, 120))
		{
			cars << CreateCar(world, Vec2{ Random(-200, 200), -600 }, vertices, indices, { 2, 6 });
		}
	}
}