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
|
#include "Joint.h"
#include "Body.h"
#include "World.h"
#include "Constants.h"
using namespace Phy2D;
void Joint::Set(Body* b1, Body* b2, const Vec2& anchor)
{
body1 = b1;
body2 = b2;
Mat22 Rot1(body1->rotation);
Mat22 Rot2(body2->rotation);
Mat22 Rot1T = Rot1.Transpose();
Mat22 Rot2T = Rot2.Transpose();
localAnchor1 = Rot1T * (anchor - body1->position);
localAnchor2 = Rot2T * (anchor - body2->position);
P.Set(_0, _0);
softness = _0;
biasFactor = _0_2;
}
void Joint::PreStep(fixed inv_dt)
{
// Pre-compute anchors, mass matrix, and bias.
Mat22 Rot1(body1->rotation);
Mat22 Rot2(body2->rotation);
r1 = Rot1 * localAnchor1;
r2 = Rot2 * localAnchor2;
// deltaV = deltaV0 + K * impulse
// invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
Mat22 K1;
K1.col1.x = body1->invMass + body2->invMass; K1.col2.x = _0;
K1.col1.y = _0; K1.col2.y = body1->invMass + body2->invMass;
Mat22 K2;
K2.col1.x = body1->invI * r1.y * r1.y; K2.col2.x = -body1->invI * r1.x * r1.y;
K2.col1.y = -body1->invI * r1.x * r1.y; K2.col2.y = body1->invI * r1.x * r1.x;
Mat22 K3;
K3.col1.x = body2->invI * r2.y * r2.y; K3.col2.x = -body2->invI * r2.x * r2.y;
K3.col1.y = -body2->invI * r2.x * r2.y; K3.col2.y = body2->invI * r2.x * r2.x;
Mat22 K = K1 + K2 + K3;
K.col1.x += softness;
K.col2.y += softness;
M = K.Invert();
Vec2 p1 = body1->position + r1;
Vec2 p2 = body2->position + r2;
Vec2 dp = p2 - p1;
if (World::positionCorrection)
{
bias = -biasFactor * inv_dt * dp;
}
else
{
bias.Set(_0, _0);
}
if (World::warmStarting)
{
// Apply accumulated impulse.
body1->velocity -= body1->invMass * P;
body1->angularVelocity -= body1->invI * Cross(r1, P);
body2->velocity += body2->invMass * P;
body2->angularVelocity += body2->invI * Cross(r2, P);
}
else
{
P.Set(_0, _0);
}
}
void Joint::ApplyImpulse()
{
Vec2 dv = body2->velocity + Cross(body2->angularVelocity, r2) - body1->velocity - Cross(body1->angularVelocity, r1);
Vec2 impulse;
impulse = M * (bias - dv - softness * P);
body1->velocity -= body1->invMass * impulse;
body1->angularVelocity -= body1->invI * Cross(r1, impulse);
body2->velocity += body2->invMass * impulse;
body2->angularVelocity += body2->invI * Cross(r2, impulse);
P += impulse;
}
|
