Box2D.NET/src/Box2D.NET.Samples/Samples/Car.cs at 2742e71bdf00da3d492c678e8da25b525199204d · ikpil/Box2D.NET · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
// SPDX-FileCopyrightText: 2022 Erin Catto
// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com)
// SPDX-License-Identifier: MIT

using static Box2D.NET.B2Joints;
using static Box2D.NET.B2Ids;
using static Box2D.NET.B2Hulls;
using static Box2D.NET.B2Geometries;
using static Box2D.NET.B2Types;
using static Box2D.NET.B2MathFunction;
using static Box2D.NET.B2Bodies;
using static Box2D.NET.B2Shapes;
using static Box2D.NET.B2WheelJoints;
using static Box2D.NET.B2Diagnostics;

namespace Box2D.NET.Samples.Samples;

public struct Car
{
    public B2BodyId m_chassisId;
    public B2BodyId m_rearWheelId;
    public B2BodyId m_frontWheelId;
    public B2JointId m_rearAxleId;
    public B2JointId m_frontAxleId;
    public bool m_isSpawned;

    public void Spawn(B2WorldId worldId, B2Vec2 position, float scale, float hertz, float dampingRatio, float torque, B2UserData userData)
    {
        B2_ASSERT(m_isSpawned == false);

        B2_ASSERT(B2_IS_NULL(m_chassisId));
        B2_ASSERT(B2_IS_NULL(m_frontWheelId));
        B2_ASSERT(B2_IS_NULL(m_rearWheelId));

        B2Vec2[] vertices = new B2Vec2[6]
        {
            new B2Vec2(-1.5f, -0.5f), new B2Vec2(1.5f, -0.5f), new B2Vec2(1.5f, 0.0f), new B2Vec2(0.0f, 0.9f), new B2Vec2(-1.15f, 0.9f), new B2Vec2(-1.5f, 0.2f)
        };

        for (int i = 0; i < 6; ++i)
        {
            vertices[i].X *= 0.85f * scale;
            vertices[i].Y *= 0.85f * scale;
        }

        B2Hull hull = b2ComputeHull(vertices, 6);
        B2Polygon chassis = b2MakePolygon(hull, 0.15f * scale);

        B2ShapeDef shapeDef = b2DefaultShapeDef();
        shapeDef.density = 1.0f / scale;
        shapeDef.material.friction = 0.2f;

        B2Circle circle = new B2Circle(new B2Vec2(0.0f, 0.0f), 0.4f * scale);

        B2BodyDef bodyDef = b2DefaultBodyDef();
        bodyDef.type = B2BodyType.b2_dynamicBody;
        bodyDef.position = b2Add(new B2Vec2(0.0f, 1.0f * scale), position);
        m_chassisId = b2CreateBody(worldId, bodyDef);
        b2CreatePolygonShape(m_chassisId, shapeDef, chassis);

        shapeDef.density = 2.0f / scale;
        shapeDef.material.friction = 1.5f;
        shapeDef.material.rollingResistance = 0.1f;

        bodyDef.position = b2Add(new B2Vec2(-1.0f * scale, 0.35f * scale), position);
        bodyDef.allowFastRotation = true;
        m_rearWheelId = b2CreateBody(worldId, bodyDef);
        b2CreateCircleShape(m_rearWheelId, shapeDef, circle);

        bodyDef.position = b2Add(new B2Vec2(1.0f * scale, 0.4f * scale), position);
        bodyDef.allowFastRotation = true;
        m_frontWheelId = b2CreateBody(worldId, bodyDef);
        b2CreateCircleShape(m_frontWheelId, shapeDef, circle);

        B2Vec2 axis = new B2Vec2(0.0f, 1.0f);
        B2Vec2 pivot = b2Body_GetPosition(m_rearWheelId);

        // float throttle = 0.0f;
        // float speed = 35.0f;
        // float torque = 2.5f * scale;
        // float hertz = 5.0f;
        // float dampingRatio = 0.7f;

        B2WheelJointDef jointDef = b2DefaultWheelJointDef();

        jointDef.@base.bodyIdA = m_chassisId;
        jointDef.@base.bodyIdB = m_rearWheelId;
        jointDef.@base.localFrameA.q = b2MakeRot(0.5f * B2_PI);
        jointDef.@base.localFrameA.p = b2Body_GetLocalPoint(jointDef.@base.bodyIdA, pivot);
        jointDef.@base.localFrameB.p = b2Body_GetLocalPoint(jointDef.@base.bodyIdB, pivot);
        jointDef.motorSpeed = 0.0f;
        jointDef.maxMotorTorque = torque;
        jointDef.enableMotor = true;
        jointDef.hertz = hertz;
        jointDef.dampingRatio = dampingRatio;
        jointDef.lowerTranslation = -0.25f * scale;
        jointDef.upperTranslation = 0.25f * scale;
        jointDef.enableLimit = true;
        m_rearAxleId = b2CreateWheelJoint(worldId, jointDef);

        pivot = b2Body_GetPosition(m_frontWheelId);
        jointDef.@base.bodyIdA = m_chassisId;
        jointDef.@base.bodyIdB = m_frontWheelId;
        jointDef.@base.localFrameA.q = b2MakeRot(0.5f * B2_PI);
        jointDef.@base.localFrameA.p = b2Body_GetLocalPoint(jointDef.@base.bodyIdA, pivot);
        jointDef.@base.localFrameB.p = b2Body_GetLocalPoint(jointDef.@base.bodyIdB, pivot);
        jointDef.motorSpeed = 0.0f;
        jointDef.maxMotorTorque = torque;
        jointDef.enableMotor = true;
        jointDef.hertz = hertz;
        jointDef.dampingRatio = dampingRatio;
        jointDef.lowerTranslation = -0.25f * scale;
        jointDef.upperTranslation = 0.25f * scale;
        jointDef.enableLimit = true;
        m_frontAxleId = b2CreateWheelJoint(worldId, jointDef);
    }

    public void Despawn()
    {
        B2_ASSERT(m_isSpawned == true);

        b2DestroyJoint(m_rearAxleId, false);
        b2DestroyJoint(m_frontAxleId, false);
        b2DestroyBody(m_rearWheelId);
        b2DestroyBody(m_frontWheelId);
        b2DestroyBody(m_chassisId);

        m_isSpawned = false;
    }

    public void SetSpeed(float speed)
    {
        b2WheelJoint_SetMotorSpeed(m_rearAxleId, speed);
        b2WheelJoint_SetMotorSpeed(m_frontAxleId, speed);
        b2Joint_WakeBodies(m_rearAxleId);
    }

    public void SetTorque(float torque)
    {
        b2WheelJoint_SetMaxMotorTorque(m_rearAxleId, torque);
        b2WheelJoint_SetMaxMotorTorque(m_frontAxleId, torque);
    }

    public void SetHertz(float hertz)
    {
        b2WheelJoint_SetSpringHertz(m_rearAxleId, hertz);
        b2WheelJoint_SetSpringHertz(m_frontAxleId, hertz);
    }

    public void SetDampingRadio(float dampingRatio)
    {
        b2WheelJoint_SetSpringDampingRatio(m_rearAxleId, dampingRatio);
        b2WheelJoint_SetSpringDampingRatio(m_frontAxleId, dampingRatio);
    }
}