void Body::UpdateFrame() { if (!(GetFlags() & Body::FLAG_CAN_MOVE_FRAME)) return; // falling out of frames if (!GetFrame()->IsLocalPosInFrame(GetPosition())) { printf("%s leaves frame %s\n", GetLabel().c_str(), GetFrame()->GetLabel().c_str()); Frame *new_frame = GetFrame()->m_parent; if (new_frame) { // don't let fall out of root frame matrix4x4d m = matrix4x4d::Identity(); GetFrame()->ApplyLeavingTransform(m); vector3d new_pos = m * GetPosition(); matrix4x4d rot; GetRotMatrix(rot); SetRotMatrix(m * rot); m.ClearToRotOnly(); SetVelocity(GetFrame()->GetVelocity() + m*(GetVelocity() - GetFrame()->GetStasisVelocityAtPosition(GetPosition()))); SetFrame(new_frame); SetPosition(new_pos); Pi::luaOnFrameChanged->Queue(this); return; } } // entering into frames for (std::list<Frame*>::iterator j = GetFrame()->m_children.begin(); j != GetFrame()->m_children.end(); ++j) { Frame *kid = *j; matrix4x4d m; Frame::GetFrameTransform(GetFrame(), kid, m); vector3d pos = m * GetPosition(); if (!kid->IsLocalPosInFrame(pos)) continue; printf("%s enters frame %s\n", GetLabel().c_str(), kid->GetLabel().c_str()); SetPosition(pos); SetFrame(kid); matrix4x4d rot; GetRotMatrix(rot); SetRotMatrix(m * rot); // get rid of transforms m.ClearToRotOnly(); SetVelocity(m*(GetVelocity() - kid->GetVelocity()) + kid->GetStasisVelocityAtPosition(pos)); Pi::luaOnFrameChanged->Queue(this); break; } }