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();
}
bool FaceModel::getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const {
if (!isActive()) {
return false;
}
const FBXGeometry& geometry = _geometry->getFBXGeometry();
return getJointPositionInWorldFrame(geometry.leftEyeJointIndex, firstEyePosition) &&
getJointPositionInWorldFrame(geometry.rightEyeJointIndex, secondEyePosition);
}
bool SkeletonModel::getNeckPosition(glm::vec3& neckPosition) const {
if (_owningAvatar->isMyAvatar() &&
Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
return isActive() && getVisibleJointPositionInWorldFrame(_geometry->getFBXGeometry().neckJointIndex, neckPosition);
}
return isActive() && getJointPositionInWorldFrame(_geometry->getFBXGeometry().neckJointIndex, neckPosition);
}
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::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;
}
bool SkeletonModel::getNeckPosition(glm::vec3& neckPosition) const {
return isActive() && getJointPositionInWorldFrame(getFBXGeometry().neckJointIndex, neckPosition);
}
bool SkeletonModel::getHeadPosition(glm::vec3& headPosition) const {
return isActive() && getJointPositionInWorldFrame(getFBXGeometry().headJointIndex, headPosition);
}
bool SkeletonModel::getRightShoulderPosition(glm::vec3& position) const {
return getJointPositionInWorldFrame(getLastFreeJointIndex(getRightHandJointIndex()), position);
}
bool SkeletonModel::getRightHandPosition(glm::vec3& position) const {
return getJointPositionInWorldFrame(getRightHandJointIndex(), position);
}
bool SkeletonModel::getNeckPosition(glm::vec3& neckPosition) const {
return isActive() && getJointPositionInWorldFrame(_rig.indexOfJoint("Neck"), neckPosition);
}
bool SkeletonModel::getHeadPosition(glm::vec3& headPosition) const {
return isActive() && getJointPositionInWorldFrame(_rig.indexOfJoint("Head"), headPosition);
}
void Flow::calculateConstraints(const std::shared_ptr<AnimSkeleton>& skeleton,
AnimPoseVec& relativePoses, AnimPoseVec& absolutePoses) {
cleanUp();
if (!skeleton) {
return;
}
auto flowPrefix = FLOW_JOINT_PREFIX.toUpper();
auto simPrefix = SIM_JOINT_PREFIX.toUpper();
std::vector<int> handsIndices;
_groupSettings.clear();
for (int i = 0; i < skeleton->getNumJoints(); i++) {
auto name = skeleton->getJointName(i);
if (std::find(HAND_COLLISION_JOINTS.begin(), HAND_COLLISION_JOINTS.end(), name) != HAND_COLLISION_JOINTS.end()) {
handsIndices.push_back(i);
}
auto parentIndex = skeleton->getParentIndex(i);
if (parentIndex == -1) {
continue;
}
auto jointChildren = skeleton->getChildrenOfJoint(i);
// auto childIndex = jointChildren.size() > 0 ? jointChildren[0] : -1;
auto group = QStringRef(&name, 0, 3).toString().toUpper();
auto split = name.split("_");
bool isSimJoint = (group == simPrefix);
bool isFlowJoint = split.size() > 2 && split[0].toUpper() == flowPrefix;
if (isFlowJoint || isSimJoint) {
group = "";
if (isSimJoint) {
for (int j = 1; j < name.size() - 1; j++) {
bool toFloatSuccess;
QStringRef(&name, (int)(name.size() - j), 1).toString().toFloat(&toFloatSuccess);
if (!toFloatSuccess && (name.size() - j) > (int)simPrefix.size()) {
group = QStringRef(&name, (int)simPrefix.size(), (int)(name.size() - j + 1) - (int)simPrefix.size()).toString();
break;
}
}
if (group.isEmpty()) {
group = QStringRef(&name, (int)simPrefix.size(), name.size() - (int)simPrefix.size()).toString();
}
qCDebug(animation) << "Sim joint added to flow: " << name;
} else {
group = split[1];
}
if (!group.isEmpty()) {
_flowJointKeywords.push_back(group);
FlowPhysicsSettings jointSettings;
if (PRESET_FLOW_DATA.find(group) != PRESET_FLOW_DATA.end()) {
jointSettings = PRESET_FLOW_DATA.at(group);
} else {
jointSettings = DEFAULT_JOINT_SETTINGS;
}
if (_flowJointData.find(i) == _flowJointData.end()) {
auto flowJoint = FlowJoint(i, parentIndex, -1, name, group, jointSettings);
_flowJointData.insert(std::pair<int, FlowJoint>(i, flowJoint));
}
updateGroupSettings(group, jointSettings);
}
} else {
if (PRESET_COLLISION_DATA.find(name) != PRESET_COLLISION_DATA.end()) {
_collisionSystem.addCollisionSphere(i, PRESET_COLLISION_DATA.at(name));
}
}
}
for (auto &jointData : _flowJointData) {
int jointIndex = jointData.first;
glm::vec3 jointPosition, parentPosition, jointTranslation;
glm::quat jointRotation;
getJointPositionInWorldFrame(absolutePoses, jointIndex, jointPosition, _entityPosition, _entityRotation);
getJointTranslation(relativePoses, jointIndex, jointTranslation);
getJointRotation(relativePoses, jointIndex, jointRotation);
getJointPositionInWorldFrame(absolutePoses, jointData.second.getParentIndex(), parentPosition, _entityPosition, _entityRotation);
jointData.second.setInitialData(jointPosition, jointTranslation, jointRotation, parentPosition);
}
std::vector<int> roots;
for (auto &joint :_flowJointData) {
if (_flowJointData.find(joint.second.getParentIndex()) == _flowJointData.end()) {
joint.second.setAnchored(true);
roots.push_back(joint.first);
} else {
_flowJointData[joint.second.getParentIndex()].setChildIndex(joint.first);
}
}
int extraIndex = -1;
for (size_t i = 0; i < roots.size(); i++) {
FlowThread thread = FlowThread(roots[i], &_flowJointData);
// add threads with at least 2 joints
if (thread._joints.size() > 0) {
if (thread._joints.size() == 1) {
int jointIndex = roots[i];
auto &joint = _flowJointData[jointIndex];
auto &jointPosition = joint.getUpdatedPosition();
auto newSettings = joint.getSettings();
extraIndex = extraIndex > -1 ? extraIndex + 1 : skeleton->getNumJoints();
joint.setChildIndex(extraIndex);
auto newJoint = FlowJoint(extraIndex, jointIndex, -1, joint.getName(), joint.getGroup(), newSettings);
newJoint.toHelperJoint(jointPosition, HELPER_JOINT_LENGTH);
glm::vec3 translation = glm::vec3(0.0f, HELPER_JOINT_LENGTH, 0.0f);
newJoint.setInitialData(jointPosition + translation, 100.0f * translation , Quaternions::IDENTITY, jointPosition);
_flowJointData.insert(std::pair<int, FlowJoint>(extraIndex, newJoint));
FlowThread newThread = FlowThread(jointIndex, &_flowJointData);
if (newThread._joints.size() > 1) {
_jointThreads.push_back(newThread);
}
} else {
_jointThreads.push_back(thread);
}
}
}
if (_jointThreads.size() == 0) {
onCleanup();
}
if (handsIndices.size() > 0) {
FlowCollisionSettings handSettings;
handSettings._radius = HAND_COLLISION_RADIUS;
for (size_t i = 0; i < handsIndices.size(); i++) {
_collisionSystem.addCollisionSphere(handsIndices[i], handSettings, glm::vec3(), true, true);
}
}
_initialized = _jointThreads.size() > 0;
}