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
|
const bool ogpRigidBody::Update(float dt)
{
const float fHalfdt = 0.5f * dt;
const float fSixthdt = dt / 6.f;
og3DVector x(0.f), v(0.f), L(0.f), w(0.f), p(0.f);
ogQuaternion q;
ogMatrix mRot = ogMatrixIdentity();
//InternalForces werden nur im ersten Schritt verwendet, da es sich um RepulsiveForces handelt!
//A1 = G(t, S0), B1 = S0 + (dt / 2) * A1 => S. 238
const og3DVector A1dxdt = m_v; //A1 dx/dt = v
const ogQuaternion A1dqdt = 0.5f * ogQuaternion(m_w) * m_q;
const og3DVector A1dpdt = m_vInternalForce + m_vExternalForce;
const og3DVector A1dLdt = m_vInternalTorque + m_vExternalTorque;
x = m_x + fHalfdt * A1dxdt;
q = m_q + fHalfdt * A1dqdt;
p = m_p + fHalfdt * A1dpdt;
L = m_L + fHalfdt * A1dLdt;
ComputeDerivations(q, p, L, &mRot, &v, &w);
//A2 = G(t + dt / 2, B1), B2 = S0 + (dt / 2) * A2
const og3DVector A2dxdt = v;
const ogQuaternion A2dqdt = 0.5f * ogQuaternion(w) * q;
const og3DVector A2dpdt = m_vExternalForce;
const og3DVector A2dLdt = m_vExternalTorque;
x = m_x + fHalfdt * A2dxdt;
q = m_q + fHalfdt * A2dqdt;
p = m_p + fHalfdt * A2dpdt;
L = m_L + fHalfdt * A2dLdt;
ComputeDerivations(q, p, L, &mRot, &v, &w);
//A3 = G(t + dt / 2, B2), B3 = S0 + dt * A3
const og3DVector A3dxdt = v;
const ogQuaternion A3dqdt = 0.5f * ogQuaternion(w) * q;
const og3DVector A3dpdt = m_vExternalForce;
const og3DVector A3dLdt = m_vExternalTorque;
x = m_x + dt * A3dxdt;
q = m_q + dt * A3dqdt;
p = m_p + dt * A3dpdt;
L = m_L + dt * A3dLdt;
ComputeDerivations(q, p, L, &mRot, &v, &w);
//A4 = G(t + dt, B3), S1 = S0 + (dt / 6) * (A1 + 2 * A2 + 2 * A3 + A4)
const og3DVector A4dxdt = v;
const ogQuaternion A4dqdt = 0.5f * ogQuaternion(w) * q;
const og3DVector A4dpdt = m_vExternalForce;
const og3DVector A4dLdt = m_vExternalTorque;
m_x = m_x + fSixthdt * (A1dxdt + 2.f * (A2dxdt + A3dxdt) + A4dxdt);
m_q = m_q + fSixthdt * (A1dqdt + 2.f * (A2dqdt + A3dqdt) + A4dqdt);
m_p = m_p + fSixthdt * (A1dpdt + 2.f * (A2dpdt + A3dpdt) + A4dpdt);
m_L = m_L + fSixthdt * (A1dLdt + 2.f * (A2dLdt + A3dLdt) + A4dLdt);
//Rotationsquaternion normalisieren!
m_q = m_q.Normalize(); //Wegen numerischen Fehler bei der Integration!
ComputeDerivations();
//Transformationsmatrizen berechnen und anwenden, dann AABB aktualisieren
ComputeTransformationMatrices();
UpdateAABBFast();
//Nach diesem Frame Externe und Interne Kräfte wieder auf Null setzen
m_vInternalForce = m_vInternalTorque = og3DVector(0.f);
m_vExternalForce = m_vExternalTorque = og3DVector(0.f);
return true;
}
|