void Flow::updateJoints(AnimPoseVec& relativePoses, AnimPoseVec& absolutePoses) {
updateAbsolutePoses(relativePoses, absolutePoses);
for (auto &jointData : _flowJointData) {
int jointIndex = jointData.first;
glm::vec3 jointPosition, parentPosition, jointTranslation;
glm::quat jointRotation, parentWorldRotation;
if (!jointData.second.isHelper()) {
getJointPositionInWorldFrame(absolutePoses, jointIndex, jointPosition, _entityPosition, _entityRotation);
getJointTranslation(relativePoses, jointIndex, jointTranslation);
getJointRotation(relativePoses, jointIndex, jointRotation);
} else {
jointPosition = jointData.second.getCurrentPosition();
jointTranslation = jointData.second.getCurrentTranslation();
jointRotation = jointData.second.getCurrentRotation();
}
getJointPositionInWorldFrame(absolutePoses, jointData.second.getParentIndex(), parentPosition, _entityPosition, _entityRotation);
getJointRotationInWorldFrame(absolutePoses, jointData.second.getParentIndex(), parentWorldRotation, _entityRotation);
jointData.second.setUpdatedData(jointPosition, jointTranslation, jointRotation, parentPosition, parentWorldRotation);
}
auto &selfCollisions = _collisionSystem.getSelfCollisions();
for (auto &collision : selfCollisions) {
updateCollisionJoint(collision, absolutePoses);
}
auto &selfTouchCollisions = _collisionSystem.getSelfTouchCollisions();
for (auto &collision : selfTouchCollisions) {
updateCollisionJoint(collision, absolutePoses);
}
_collisionSystem.prepareCollisions();
}
void Flow::updateCollisionJoint(FlowCollisionSphere& collision, AnimPoseVec& absolutePoses) {
glm::quat jointRotation;
getJointPositionInWorldFrame(absolutePoses, collision._jointIndex, collision._position, _entityPosition, _entityRotation);
getJointRotationInWorldFrame(absolutePoses, collision._jointIndex, jointRotation, _entityRotation);
glm::vec3 worldOffset = jointRotation * collision._offset;
collision._position = collision._position + worldOffset;
}
bool Flow::worldToJointPoint(const AnimPoseVec& absolutePoses, const glm::vec3& position, const int jointIndex, glm::vec3& jointSpacePosition) const {
glm::vec3 jointPos;
glm::quat jointRot;
if (getJointPositionInWorldFrame(absolutePoses, jointIndex, jointPos, _entityPosition, _entityRotation) &&
getJointRotationInWorldFrame(absolutePoses, jointIndex, jointRot, _entityRotation)) {
glm::vec3 modelOffset = position - jointPos;
jointSpacePosition = glm::inverse(jointRot) * modelOffset;
return true;
}
return false;
}
bool SkeletonModel::getNeckParentRotationFromDefaultOrientation(glm::quat& neckParentRotation) const {
if (!isActive()) {
return false;
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
if (geometry.neckJointIndex == -1) {
return false;
}
int parentIndex = geometry.joints.at(geometry.neckJointIndex).parentIndex;
glm::quat worldFrameRotation;
bool success = getJointRotationInWorldFrame(parentIndex, worldFrameRotation);
if (success) {
neckParentRotation = worldFrameRotation * _jointStates[parentIndex].getFBXJoint().inverseDefaultRotation;
}
return success;
}
bool SkeletonModel::getNeckParentRotationFromDefaultOrientation(glm::quat& neckParentRotation) const {
if (!isActive()) {
return false;
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
if (geometry.neckJointIndex == -1) {
return false;
}
int parentIndex = geometry.joints.at(geometry.neckJointIndex).parentIndex;
glm::quat worldFrameRotation;
bool success = false;
if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
success = getVisibleJointRotationInWorldFrame(parentIndex, worldFrameRotation);
} else {
success = getJointRotationInWorldFrame(parentIndex, worldFrameRotation);
}
if (success) {
neckParentRotation = worldFrameRotation * _jointStates[parentIndex].getFBXJoint().inverseDefaultRotation;
}
return success;
}
bool SkeletonModel::getRightGrabPosition(glm::vec3& position) const {
int knuckleIndex = _rig->indexOfJoint("RightHandMiddle1");
int handIndex = _rig->indexOfJoint("RightHand");
if (knuckleIndex >= 0 && handIndex >= 0) {
glm::quat handRotation;
glm::vec3 knucklePosition;
glm::vec3 handPosition;
if (!getJointPositionInWorldFrame(knuckleIndex, knucklePosition)) {
return false;
}
if (!getJointPositionInWorldFrame(handIndex, handPosition)) {
return false;
}
if (!getJointRotationInWorldFrame(handIndex, handRotation)) {
return false;
}
float halfPalmLength = glm::distance(knucklePosition, handPosition) * 0.5f;
// z azis is standardized to be out of the palm. move from the knuckle-joint away from the palm
// by 1/2 the palm length.
position = knucklePosition + handRotation * (glm::vec3(0.0f, 0.0f, 1.0f) * halfPalmLength);
return true;
}
return false;
}
