// // Flow.cpp // // Created by Luis Cuenca on 1/21/2019. // Copyright 2019 High Fidelity, Inc. // // Distributed under the Apache License, Version 2.0. // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // #include "Flow.h" #include "Rig.h" #include "AnimSkeleton.h" const std::map PRESET_FLOW_DATA = { { "hair", FlowPhysicsSettings() }, { "skirt", FlowPhysicsSettings(true, 1.0f, DEFAULT_GRAVITY, 0.65f, 0.8f, 0.45f, 0.01f) }, { "breast", FlowPhysicsSettings(true, 1.0f, DEFAULT_GRAVITY, 0.65f, 0.8f, 0.45f, 0.01f) } }; const std::map PRESET_COLLISION_DATA = { { "Spine2", FlowCollisionSettings(QUuid(), FlowCollisionType::CollisionSphere, glm::vec3(0.0f, 0.2f, 0.0f), 0.14f) }, { "LeftArm", FlowCollisionSettings(QUuid(), FlowCollisionType::CollisionSphere, glm::vec3(0.0f, 0.02f, 0.0f), 0.05f) }, { "RightArm", FlowCollisionSettings(QUuid(), FlowCollisionType::CollisionSphere, glm::vec3(0.0f, 0.02f, 0.0f), 0.05f) }, { "HeadTop_End", FlowCollisionSettings(QUuid(), FlowCollisionType::CollisionSphere, glm::vec3(0.0f, -0.15f, 0.0f), 0.09f) } }; FlowCollisionSphere::FlowCollisionSphere(const int& jointIndex, const FlowCollisionSettings& settings, bool isTouch) { _jointIndex = jointIndex; _radius = _initialRadius = settings._radius; _offset = _initialOffset = settings._offset; _entityID = settings._entityID; _isTouch = isTouch; } FlowCollisionResult FlowCollisionSphere::computeSphereCollision(const glm::vec3& point, float radius) const { FlowCollisionResult result; auto centerToJoint = point - _position; result._distance = glm::length(centerToJoint) - radius; result._offset = _radius - result._distance; result._normal = glm::normalize(centerToJoint); result._radius = _radius; result._position = _position; return result; } FlowCollisionResult FlowCollisionSphere::checkSegmentCollision(const glm::vec3& point1, const glm::vec3& point2, const FlowCollisionResult& collisionResult1, const FlowCollisionResult& collisionResult2) { FlowCollisionResult result; auto segment = point2 - point1; auto segmentLength = glm::length(segment); auto maxDistance = glm::sqrt(powf(collisionResult1._radius, 2.0f) + powf(segmentLength, 2.0f)); if (collisionResult1._distance < maxDistance && collisionResult2._distance < maxDistance) { float segmentPercentage = collisionResult1._distance / (collisionResult1._distance + collisionResult2._distance); glm::vec3 collisionPoint = point1 + segment * segmentPercentage; glm::vec3 centerToSegment = collisionPoint - _position; float distance = glm::length(centerToSegment); if (distance < _radius) { result._offset = _radius - distance; result._position = _position; result._radius = _radius; result._normal = glm::normalize(centerToSegment); result._distance = distance; } } return result; } void FlowCollisionSystem::addCollisionSphere(int jointIndex, const FlowCollisionSettings& settings, const glm::vec3& position, bool isSelfCollision, bool isTouch) { auto collision = FlowCollisionSphere(jointIndex, settings, isTouch); collision.setPosition(position); if (isSelfCollision) { if (!isTouch) { _selfCollisions.push_back(collision); } else { _selfTouchCollisions.push_back(collision); } } else { _othersCollisions.push_back(collision); } }; void FlowCollisionSystem::resetCollisions() { _allCollisions.clear(); _othersCollisions.clear(); _selfTouchCollisions.clear(); _selfCollisions.clear(); } FlowCollisionResult FlowCollisionSystem::computeCollision(const std::vector collisions) { FlowCollisionResult result; if (collisions.size() > 1) { for (size_t i = 0; i < collisions.size(); i++) { result._offset += collisions[i]._offset; result._normal = result._normal + collisions[i]._normal * collisions[i]._distance; result._position = result._position + collisions[i]._position; result._radius += collisions[i]._radius; result._distance += collisions[i]._distance; } result._offset = result._offset / collisions.size(); result._radius = 0.5f * glm::length(result._normal); result._normal = glm::normalize(result._normal); result._position = result._position / (float)collisions.size(); result._distance = result._distance / collisions.size(); } else if (collisions.size() == 1) { result = collisions[0]; } result._count = (int)collisions.size(); return result; }; void FlowCollisionSystem::setScale(float scale) { _scale = scale; for (size_t j = 0; j < _selfCollisions.size(); j++) { _selfCollisions[j]._radius = _selfCollisions[j]._initialRadius * scale; _selfCollisions[j]._offset = _selfCollisions[j]._initialOffset * scale; } for (size_t j = 0; j < _selfTouchCollisions.size(); j++) { _selfTouchCollisions[j]._radius = _selfTouchCollisions[j]._initialRadius * scale; _selfTouchCollisions[j]._offset = _selfTouchCollisions[j]._initialOffset * scale; } }; std::vector FlowCollisionSystem::checkFlowThreadCollisions(FlowThread* flowThread) { std::vector> FlowThreadResults; FlowThreadResults.resize(flowThread->_joints.size()); for (size_t j = 0; j < _allCollisions.size(); j++) { FlowCollisionSphere &sphere = _allCollisions[j]; FlowCollisionResult rootCollision = sphere.computeSphereCollision(flowThread->_positions[0], flowThread->_radius); std::vector collisionData = { rootCollision }; bool tooFar = rootCollision._distance >(flowThread->_length + rootCollision._radius); FlowCollisionResult nextCollision; if (!tooFar) { if (sphere._isTouch) { for (size_t i = 1; i < flowThread->_joints.size(); i++) { auto prevCollision = collisionData[i - 1]; nextCollision = _allCollisions[j].computeSphereCollision(flowThread->_positions[i], flowThread->_radius); collisionData.push_back(nextCollision); if (prevCollision._offset > 0.0f) { if (i == 1) { FlowThreadResults[i - 1].push_back(prevCollision); } } else if (nextCollision._offset > 0.0f) { FlowThreadResults[i].push_back(nextCollision); } else { FlowCollisionResult segmentCollision = _allCollisions[j].checkSegmentCollision(flowThread->_positions[i - 1], flowThread->_positions[i], prevCollision, nextCollision); if (segmentCollision._offset > 0) { FlowThreadResults[i - 1].push_back(segmentCollision); FlowThreadResults[i].push_back(segmentCollision); } } } } else { if (rootCollision._offset > 0.0f) { FlowThreadResults[0].push_back(rootCollision); } for (size_t i = 1; i < flowThread->_joints.size(); i++) { nextCollision = _allCollisions[j].computeSphereCollision(flowThread->_positions[i], flowThread->_radius); if (nextCollision._offset > 0.0f) { FlowThreadResults[i].push_back(nextCollision); } } } } } std::vector results; for (size_t i = 0; i < flowThread->_joints.size(); i++) { results.push_back(computeCollision(FlowThreadResults[i])); } return results; }; FlowCollisionSettings FlowCollisionSystem::getCollisionSettingsByJoint(int jointIndex) { for (auto &collision : _selfCollisions) { if (collision._jointIndex == jointIndex) { return FlowCollisionSettings(collision._entityID, FlowCollisionType::CollisionSphere, collision._initialOffset, collision._initialRadius); } } return FlowCollisionSettings(); } void FlowCollisionSystem::setCollisionSettingsByJoint(int jointIndex, const FlowCollisionSettings& settings) { for (auto &collision : _selfCollisions) { if (collision._jointIndex == jointIndex) { collision._initialRadius = settings._radius; collision._initialOffset = settings._offset; collision._radius = _scale * settings._radius; collision._offset = _scale * settings._offset; } } } void FlowCollisionSystem::prepareCollisions() { _allCollisions.clear(); _allCollisions.insert(_allCollisions.end(), _selfCollisions.begin(), _selfCollisions.end()); _allCollisions.insert(_allCollisions.end(), _othersCollisions.begin(), _othersCollisions.end()); _allCollisions.insert(_allCollisions.end(), _selfTouchCollisions.begin(), _selfTouchCollisions.end()); _othersCollisions.clear(); } FlowNode::FlowNode(const glm::vec3& initialPosition, FlowPhysicsSettings settings) { _initialPosition = _previousPosition = _currentPosition = initialPosition; _initialRadius = settings._radius; } void FlowNode::update(float deltaTime, const glm::vec3& accelerationOffset) { _acceleration = glm::vec3(0.0f, _settings._gravity, 0.0f); _previousVelocity = _currentVelocity; _currentVelocity = _currentPosition - _previousPosition; _previousPosition = _currentPosition; if (!_anchored) { // Add inertia const float FPS = 60.0f; float timeRatio = _scale * (FPS * deltaTime); float invertedTimeRatio = timeRatio > 0.0f ? 1.0f / timeRatio : 1.0f; auto deltaVelocity = _previousVelocity - _currentVelocity; auto centrifugeVector = glm::length(deltaVelocity) != 0.0f ? glm::normalize(deltaVelocity) : glm::vec3(); _acceleration = _acceleration + centrifugeVector * _settings._inertia * glm::length(_currentVelocity) * invertedTimeRatio; // Add offset _acceleration += accelerationOffset; float accelerationFactor = powf(_settings._delta, 2.0f) * timeRatio; glm::vec3 deltaAcceleration = _acceleration * accelerationFactor; // Calculate new position _currentPosition = _currentPosition + (_currentVelocity * _settings._damping) + deltaAcceleration; } else { _acceleration = glm::vec3(0.0f); _currentVelocity = glm::vec3(0.0f); } }; void FlowNode::solve(const glm::vec3& constrainPoint, float maxDistance, const FlowCollisionResult& collision) { solveConstraints(constrainPoint, maxDistance); solveCollisions(collision); }; void FlowNode::solveConstraints(const glm::vec3& constrainPoint, float maxDistance) { glm::vec3 constrainVector = _currentPosition - constrainPoint; float difference = maxDistance / glm::length(constrainVector); _currentPosition = difference < 1.0f ? constrainPoint + constrainVector * difference : _currentPosition; }; void FlowNode::solveCollisions(const FlowCollisionResult& collision) { _colliding = collision._offset > 0.0f; _collision = collision; if (_colliding) { _currentPosition = _currentPosition + collision._normal * collision._offset; _previousCollision = collision; } else { _previousCollision = FlowCollisionResult(); } }; FlowJoint::FlowJoint(int jointIndex, int parentIndex, int childIndex, const QString& name, const QString& group, const FlowPhysicsSettings& settings) { _index = jointIndex; _name = name; _group = group; _childIndex = childIndex; _parentIndex = parentIndex; FlowNode(glm::vec3(), settings); }; void FlowJoint::setInitialData(const glm::vec3& initialPosition, const glm::vec3& initialTranslation, const glm::quat& initialRotation, const glm::vec3& parentPosition) { _initialPosition = initialPosition; _previousPosition = initialPosition; _currentPosition = initialPosition; _initialTranslation = initialTranslation; _currentRotation = initialRotation; _initialRotation = initialRotation; _translationDirection = glm::normalize(_initialTranslation); _parentPosition = parentPosition; _initialLength = _length = glm::length(_initialPosition - parentPosition); } void FlowJoint::setUpdatedData(const glm::vec3& updatedPosition, const glm::vec3& updatedTranslation, const glm::quat& updatedRotation, const glm::vec3& parentPosition, const glm::quat& parentWorldRotation) { _updatedPosition = updatedPosition; _updatedRotation = updatedRotation; _updatedTranslation = updatedTranslation; _parentPosition = parentPosition; _parentWorldRotation = parentWorldRotation; } void FlowJoint::setRecoveryPosition(const glm::vec3& recoveryPosition) { _recoveryPosition = recoveryPosition; _applyRecovery = true; } void FlowJoint::update(float deltaTime) { if (_settings._active) { glm::vec3 accelerationOffset = glm::vec3(0.0f); if (_settings._stiffness > 0.0f) { glm::vec3 recoveryVector = _recoveryPosition - _currentPosition; float recoveryFactor = powf(_settings._stiffness, 3.0f); accelerationOffset = recoveryVector * recoveryFactor; } FlowNode::update(deltaTime, accelerationOffset); if (_anchored) { if (!_isHelper) { _currentPosition = _updatedPosition; } else { _currentPosition = _parentPosition; } } } }; void FlowJoint::setScale(float scale, bool initScale) { if (initScale) { _initialLength = _length / scale; } _settings._radius = _initialRadius * scale; _length = _initialLength * scale; _scale = scale; } void FlowJoint::solve(const FlowCollisionResult& collision) { FlowNode::solve(_parentPosition, _length, collision); }; void FlowJoint::toHelperJoint(const glm::vec3& initialPosition, float length) { _initialPosition = initialPosition; _isHelper = true; _length = length; } FlowThread::FlowThread(int rootIndex, std::map* joints, float rigScale) { _rigScale = rigScale; _jointsPointer = joints; computeFlowThread(rootIndex); } void FlowThread::resetLength() { _length = 0.0f; for (size_t i = 1; i < _joints.size(); i++) { int index = _joints[i]; _length += _jointsPointer->at(index)._length; } } void FlowThread::computeFlowThread(int rootIndex) { int parentIndex = rootIndex; if (_jointsPointer->size() == 0) { return; } int childIndex = _jointsPointer->at(parentIndex)._childIndex; std::vector indexes = { parentIndex }; for (size_t i = 0; i < _jointsPointer->size(); i++) { if (childIndex > -1) { indexes.push_back(childIndex); childIndex = _jointsPointer->at(childIndex)._childIndex; } else { break; } } _length = 0.0f; for (size_t i = 0; i < indexes.size(); i++) { int index = indexes[i]; _joints.push_back(index); if (i > 0) { _length += _jointsPointer->at(index)._length; } } }; void FlowThread::computeRecovery() { int parentIndex = _joints[0]; auto parentJoint = _jointsPointer->at(parentIndex); _jointsPointer->at(parentIndex)._recoveryPosition = parentJoint._recoveryPosition = parentJoint._currentPosition; for (size_t i = 1; i < _joints.size(); i++) { glm::quat parentRotation = parentJoint._parentWorldRotation * parentJoint._initialRotation; auto joint = _jointsPointer->at(_joints[i]); _jointsPointer->at(_joints[i])._recoveryPosition = joint._recoveryPosition = parentJoint._recoveryPosition + (parentRotation * (joint._initialTranslation * _rigScale)); parentJoint = joint; } }; void FlowThread::update(float deltaTime) { _positions.clear(); auto &firstJoint = _jointsPointer->at(_joints[0]); _radius = firstJoint._settings._radius; computeRecovery(); for (size_t i = 0; i < _joints.size(); i++) { auto &joint = _jointsPointer->at(_joints[i]); joint.update(deltaTime); _positions.push_back(joint._currentPosition); } }; void FlowThread::solve(FlowCollisionSystem& collisionSystem) { if (collisionSystem.getActive()) { auto bodyCollisions = collisionSystem.checkFlowThreadCollisions(this); for (size_t i = 0; i < _joints.size(); i++) { int index = _joints[i]; _jointsPointer->at(index).solve(bodyCollisions[i]); } } else { for (size_t i = 0; i < _joints.size(); i++) { int index = _joints[i]; _jointsPointer->at(index).solve(FlowCollisionResult()); } } }; void FlowThread::computeJointRotations() { auto pos0 = _rootFramePositions[0]; auto pos1 = _rootFramePositions[1]; auto joint0 = _jointsPointer->at(_joints[0]); auto joint1 = _jointsPointer->at(_joints[1]); auto initial_pos1 = pos0 + (joint0._initialRotation * (joint1._initialTranslation * _rigScale)); auto vec0 = initial_pos1 - pos0; auto vec1 = pos1 - pos0; auto delta = rotationBetween(vec0, vec1); joint0._currentRotation = _jointsPointer->at(_joints[0])._currentRotation = delta * joint0._initialRotation; for (size_t i = 1; i < _joints.size() - 1; i++) { auto nextJoint = _jointsPointer->at(_joints[i + 1]); for (size_t j = i; j < _joints.size(); j++) { _rootFramePositions[j] = glm::inverse(joint0._currentRotation) * _rootFramePositions[j] - (joint0._initialTranslation * _rigScale); } pos0 = _rootFramePositions[i]; pos1 = _rootFramePositions[i + 1]; initial_pos1 = pos0 + joint1._initialRotation * (nextJoint._initialTranslation * _rigScale); vec0 = initial_pos1 - pos0; vec1 = pos1 - pos0; delta = rotationBetween(vec0, vec1); joint1._currentRotation = _jointsPointer->at(joint1._index)._currentRotation = delta * joint1._initialRotation; joint0 = joint1; joint1 = nextJoint; } } void FlowThread::setScale(float scale, bool initScale) { for (size_t i = 0; i < _joints.size(); i++) { auto &joint = _jointsPointer->at(_joints[i]); joint.setScale(scale, initScale); } resetLength(); } FlowThread& FlowThread::operator=(const FlowThread& otherFlowThread) { _rigScale = otherFlowThread._rigScale; for (int jointIndex: otherFlowThread._joints) { auto& joint = otherFlowThread._jointsPointer->at(jointIndex); auto& myJoint = _jointsPointer->at(jointIndex); myJoint._acceleration = joint._acceleration; myJoint._currentPosition = joint._currentPosition; myJoint._currentRotation = joint._currentRotation; myJoint._currentVelocity = joint._currentVelocity; myJoint._length = joint._length; myJoint._parentPosition = joint._parentPosition; myJoint._parentWorldRotation = joint._parentWorldRotation; myJoint._previousPosition = joint._previousPosition; myJoint._previousVelocity = joint._previousVelocity; myJoint._scale = joint._scale; myJoint._translationDirection = joint._translationDirection; myJoint._updatedPosition = joint._updatedPosition; myJoint._updatedRotation = joint._updatedRotation; myJoint._updatedTranslation = joint._updatedTranslation; myJoint._isHelper = joint._isHelper; } return *this; } void Flow::calculateConstraints(const std::shared_ptr& skeleton, AnimPoseVec& relativePoses, AnimPoseVec& absolutePoses) { cleanUp(); if (!skeleton) { return; } auto flowPrefix = FLOW_JOINT_PREFIX.toUpper(); auto simPrefix = SIM_JOINT_PREFIX.toUpper(); std::vector 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(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 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; qDebug() << "GetScaleFactorGeometryToUnscaledRig() " << _rig->GetScaleFactorGeometryToUnscaledRig(); for (size_t i = 0; i < roots.size(); i++) { FlowThread thread = FlowThread(roots[i], &_flowJointData, _rig->GetScaleFactorGeometryToUnscaledRig()); // 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(extraIndex, newJoint)); FlowThread newThread = FlowThread(jointIndex, &_flowJointData, _rig->GetScaleFactorGeometryToUnscaledRig()); 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; } void Flow::cleanUp() { _flowJointData.clear(); _jointThreads.clear(); _flowJointKeywords.clear(); _collisionSystem.resetCollisions(); _initialized = false; _isScaleSet = false; onCleanup(); } void Flow::setTransform(float scale, const glm::vec3& position, const glm::quat& rotation) { _scale = scale; _entityPosition = position; _entityRotation = rotation; } void Flow::setScale(float scale) { _collisionSystem.setScale(_scale); for (size_t i = 0; i < _jointThreads.size(); i++) { _jointThreads[i].setScale(_scale, !_isScaleSet); } if (_lastScale != _scale) { _lastScale = _scale; _isScaleSet = true; } } void Flow::update(float deltaTime, AnimPoseVec& relativePoses, AnimPoseVec& absolutePoses, const std::vector& overrideFlags) { if (_initialized && _active) { uint64_t startTime = usecTimestampNow(); uint64_t updateExpiry = startTime + MAX_UPDATE_FLOW_TIME_BUDGET; if (_scale != _lastScale) { setScale(_scale); } for (size_t i = 0; i < _jointThreads.size(); i++) { size_t index = _invertThreadLoop ? _jointThreads.size() - 1 - i : i; auto &thread = _jointThreads[index]; thread.update(deltaTime); thread.solve(_collisionSystem); if (!updateRootFramePositions(absolutePoses, index)) { return; } thread.computeJointRotations(); if (usecTimestampNow() > updateExpiry) { break; qWarning(animation) << "Flow Bones ran out of time while updating threads"; } } setJoints(relativePoses, overrideFlags); updateJoints(relativePoses, absolutePoses); _invertThreadLoop = !_invertThreadLoop; } } void Flow::updateAbsolutePoses(const AnimPoseVec& relativePoses, AnimPoseVec& absolutePoses) { for (auto &joint : _flowJointData) { int index = joint.second.getIndex(); int parentIndex = joint.second.getParentIndex(); if (index >= 0 && index < (int)relativePoses.size() && parentIndex >= 0 && parentIndex < (int)absolutePoses.size()) { absolutePoses[index] = absolutePoses[parentIndex] * relativePoses[index]; } } } 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 Flow::updateRootFramePositions(const AnimPoseVec& absolutePoses, size_t threadIndex) { auto &joints = _jointThreads[threadIndex]._joints; int rootIndex = _flowJointData[joints[0]].getParentIndex(); _jointThreads[threadIndex]._rootFramePositions.clear(); for (size_t j = 0; j < joints.size(); j++) { glm::vec3 jointPos; if (worldToJointPoint(absolutePoses, _flowJointData[joints[j]].getCurrentPosition(), rootIndex, jointPos)) { _jointThreads[threadIndex]._rootFramePositions.push_back(jointPos); } else { return false; } } return true; } 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; } 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::setJoints(AnimPoseVec& relativePoses, const std::vector& overrideFlags) { for (auto &thread : _jointThreads) { auto &joints = thread._joints; for (int jointIndex : joints) { auto &joint = _flowJointData[jointIndex]; if (jointIndex >= 0 && jointIndex < (int)relativePoses.size() && !overrideFlags[jointIndex]) { relativePoses[jointIndex].rot() = joint.getSettings()._active ? joint.getCurrentRotation() : joint.getInitialRotation(); } } } } void Flow::setOthersCollision(const QUuid& otherId, int jointIndex, const glm::vec3& position) { FlowCollisionSettings settings; settings._entityID = otherId; settings._radius = HAND_COLLISION_RADIUS; _collisionSystem.addCollisionSphere(jointIndex, settings, position, false, true); } void Flow::setPhysicsSettingsForGroup(const QString& group, const FlowPhysicsSettings& settings) { for (auto &joint : _flowJointData) { if (joint.second.getGroup().toUpper() == group.toUpper()) { joint.second.setSettings(settings); } } updateGroupSettings(group, settings); } bool Flow::getJointPositionInWorldFrame(const AnimPoseVec& absolutePoses, int jointIndex, glm::vec3& position, glm::vec3 translation, glm::quat rotation) const { if (jointIndex >= 0 && jointIndex < (int)absolutePoses.size()) { glm::vec3 poseSetTrans = absolutePoses[jointIndex].trans(); position = (rotation * poseSetTrans) + translation; if (!isNaN(position)) { return true; } else { position = glm::vec3(0.0f); } } return false; } bool Flow::getJointRotationInWorldFrame(const AnimPoseVec& absolutePoses, int jointIndex, glm::quat& result, const glm::quat& rotation) const { if (jointIndex >= 0 && jointIndex < (int)absolutePoses.size()) { result = rotation * absolutePoses[jointIndex].rot(); return true; } else { return false; } } bool Flow::getJointRotation(const AnimPoseVec& relativePoses, int jointIndex, glm::quat& rotation) const { if (jointIndex >= 0 && jointIndex < (int)relativePoses.size()) { rotation = relativePoses[jointIndex].rot(); return true; } else { return false; } } bool Flow::getJointTranslation(const AnimPoseVec& relativePoses, int jointIndex, glm::vec3& translation) const { if (jointIndex >= 0 && jointIndex < (int)relativePoses.size()) { translation = relativePoses[jointIndex].trans(); return true; } else { return false; } } Flow& Flow::operator=(const Flow& otherFlow) { _active = otherFlow.getActive(); _scale = otherFlow.getScale(); _isScaleSet = true; _rig = otherFlow._rig; auto &threads = otherFlow.getThreads(); if (threads.size() == _jointThreads.size()) { for (size_t i = 0; i < _jointThreads.size(); i++) { _jointThreads[i] = threads[i]; } } return *this; } void Flow::updateGroupSettings(const QString& group, const FlowPhysicsSettings& settings) { if (_groupSettings.find(group) == _groupSettings.end()) { _groupSettings.insert(std::pair(group, settings)); } else { _groupSettings[group] = settings; } }