AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

consistency

Browse Source
This commit is contained in:
Irlan
2017-06-04 13:58:13 -03:00
parent 8640a80914
commit 0b79b7283d
1 changed files with 8 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/bounce/dynamics/rope/rope.cpp
View File

@ -27,7 +27,7 @@ struct b3RopeBody
// J * v // J * v
b3MotionVec v_J() const b3MotionVec v_J() const
{ {
return m_S[0] * m_v.x + m_S[1] * m_v.y + m_S[2] * m_v.z; return m_S[0] * m_v[0] + m_S[1] * m_v[1] + m_S[2] * m_v[2];
} }
// //
@ -142,12 +142,12 @@ void b3Rope::Initialize(const b3RopeDef& def)
} }
m_v.SetZero(); m_v.SetZero();
m_w.SetZero();
m_p = def.vertices[0]; m_p = def.vertices[0];
m_w.SetZero();
m_q.SetIdentity(); m_q.SetIdentity();
m_links[0].m_X.rotation = b3QuatMat33(m_q); m_links->m_X.rotation = b3QuatMat33(m_q);
m_links[0].m_X.position = m_p; m_links->m_X.position = m_p;
for (u32 i = 1; i < m_count; ++i) for (u32 i = 1; i < m_count; ++i)
{ {
@ -414,12 +414,11 @@ void b3Rope::Step(float32 h)
{ {
b3RopeBody* b = m_links; b3RopeBody* b = m_links;
// Integrate acceleration
// Convert local to global acceleration // Convert local to global acceleration
b3Vec3 v_dot = b3Mul(m_q, b->m_sa.v); b3Vec3 v_dot = b3Mul(m_q, b->m_sa.v);
b3Vec3 w_dot = b3Mul(m_q, b->m_sa.w); b3Vec3 w_dot = b3Mul(m_q, b->m_sa.w);
// Integrate acceleration
m_v += h * v_dot; m_v += h * v_dot;
m_w += h * w_dot; m_w += h * w_dot;
@ -430,10 +429,6 @@ void b3Rope::Step(float32 h)
b3Quat q_dot = 0.5f * q_w * m_q; b3Quat q_dot = 0.5f * q_w * m_q;
m_q += h * q_dot; m_q += h * q_dot;
m_q.Normalize(); m_q.Normalize();
// Synchronize transform
b->m_X.rotation = b3QuatMat33(m_q);
b->m_X.position = m_p;
} }
for (u32 i = 1; i < m_count; ++i) for (u32 i = 1; i < m_count; ++i)
@ -446,7 +441,7 @@ void b3Rope::Step(float32 h)
// Avoid numerical instability due to large velocities // Avoid numerical instability due to large velocities
link->m_v = b3Clamp(link->m_v, min, max); link->m_v = b3Clamp(link->m_v, min, max);
// // Integrate velocity
b3Quat q_w(link->m_v.x, link->m_v.y, link->m_v.z, 0.0f); b3Quat q_w(link->m_v.x, link->m_v.y, link->m_v.z, 0.0f);
b3Quat q_dot = 0.5f * link->m_p * q_w; b3Quat q_dot = 0.5f * link->m_p * q_w;
@ -455,6 +450,8 @@ void b3Rope::Step(float32 h)
} }
// Propagate down transforms // Propagate down transforms
m_links->m_X.rotation = b3QuatMat33(m_q);
m_links->m_X.position = m_p;
m_links->m_invX = b3Inverse(m_links->m_X); m_links->m_invX = b3Inverse(m_links->m_X);
for (u32 j = 1; j < m_count; ++j) for (u32 j = 1; j < m_count; ++j)