immediately propagate forward kinematics + collision world transform, after 'INIT_POSE' command

This commit is contained in:
Erwin Coumans 2016-11-06 15:02:07 -08:00
parent 6a0f095d5b
commit c1f728ec86
2 changed files with 16 additions and 1 deletions

View File

@ -2313,6 +2313,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
dofIndex += mb->getLink(i).m_dofCount; dofIndex += mb->getLink(i).m_dofCount;
} }
} }
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q,scratch_m);
int nLinks = mb->getNumLinks();
scratch_q.resize(nLinks+1);
scratch_m.resize(nLinks+1);
mb->updateCollisionObjectWorldTransforms(scratch_q,scratch_m);
} }
SharedMemoryStatus& serverCmd =serverStatusOut; SharedMemoryStatus& serverCmd =serverStatusOut;

View File

@ -119,8 +119,11 @@ int main(int argc, char* argv[]) {
printf("Say> "); printf("Say> ");
#ifdef _WIN32
gets_s(message, 1024); gets_s(message, 1024);
#else
gets(message);
#endif
if (strcmp(message, "exit") == 0 || if (strcmp(message, "exit") == 0 ||
strcmp(message, "quit") == 0) { strcmp(message, "quit") == 0) {
break; break;