Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-12 09:42:11 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge pull request #14722 from luiscuenca/multiSphereAvatar04

Browse source 
Multi-sphere avatar detailed collisions
This commit is contained in:
Jamil Akram 2019-01-29 10:10:04 -08:00 committed by GitHub
commit f4ead69549
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
32 changed files with 1764 additions and 241 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

14
interface/src/Application.cpp
View file

@ -161,6 +161,7 @@
#include "avatar/AvatarManager.h"
#include "avatar/MyHead.h"
#include "avatar/AvatarPackager.h"
#include "avatar/MyCharacterController.h"
#include "CrashRecoveryHandler.h"
#include "CrashHandler.h"
#include "devices/DdeFaceTracker.h"
@ -2713,7 +2714,14 @@ Application::~Application() {
avatarManager->handleProcessedPhysicsTransaction(transaction);
avatarManager->deleteAllAvatars();
auto myCharacterController = getMyAvatar()->getCharacterController();
myCharacterController->clearDetailedMotionStates();
myCharacterController->buildPhysicsTransaction(transaction);
_physicsEngine->processTransaction(transaction);
myCharacterController->handleProcessedPhysicsTransaction(transaction);
_physicsEngine->setCharacterController(nullptr);
// the _shapeManager should have zero references
@ -6287,7 +6295,9 @@ void Application::update(float deltaTime) {
avatarManager->buildPhysicsTransaction(transaction);
_physicsEngine->processTransaction(transaction);
avatarManager->handleProcessedPhysicsTransaction(transaction);
myAvatar->getCharacterController()->buildPhysicsTransaction(transaction);
_physicsEngine->processTransaction(transaction);
myAvatar->getCharacterController()->handleProcessedPhysicsTransaction(transaction);
myAvatar->prepareForPhysicsSimulation();
_physicsEngine->forEachDynamic([&](EntityDynamicPointer dynamic) {
dynamic->prepareForPhysicsSimulation();

224
interface/src/avatar/AvatarManager.cpp
View file

@ -43,6 +43,7 @@
#include "InterfaceLogging.h"
#include "Menu.h"
#include "MyAvatar.h"
#include "DebugDraw.h"
#include "SceneScriptingInterface.h"
// 50 times per second - target is 45hz, but this helps account for any small deviations
@ -396,21 +397,45 @@ void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transact
if (isInPhysics) {
transaction.objectsToRemove.push_back(avatar->_motionState);
avatar->_motionState = nullptr;
auto& detailedMotionStates = avatar->getDetailedMotionStates();
for (auto& mState : detailedMotionStates) {
transaction.objectsToRemove.push_back(mState);
}
avatar->resetDetailedMotionStates();
} else {
ShapeInfo shapeInfo;
avatar->computeShapeInfo(shapeInfo);
btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
if (shape) {
AvatarMotionState* motionState = new AvatarMotionState(avatar, shape);
motionState->setMass(avatar->computeMass());
avatar->_motionState = motionState;
transaction.objectsToAdd.push_back(motionState);
if (avatar->getDetailedMotionStates().size() == 0) {
avatar->createDetailedMotionStates(avatar);
for (auto dMotionState : avatar->getDetailedMotionStates()) {
transaction.objectsToAdd.push_back(dMotionState);
}
}
if (avatar->getDetailedMotionStates().size() > 0) {
ShapeInfo shapeInfo;
avatar->computeShapeInfo(shapeInfo);
btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
if (shape) {
AvatarMotionState* motionState = new AvatarMotionState(avatar, shape);
motionState->setMass(avatar->computeMass());
avatar->_motionState = motionState;
transaction.objectsToAdd.push_back(motionState);
} else {
failedShapeBuilds.insert(avatar);
}
} else {
failedShapeBuilds.insert(avatar);
}
}
} else if (isInPhysics) {
transaction.objectsToChange.push_back(avatar->_motionState);
auto& detailedMotionStates = avatar->getDetailedMotionStates();
for (auto& mState : detailedMotionStates) {
if (mState) {
transaction.objectsToChange.push_back(mState);
}
}
}
}
_avatarsToChangeInPhysics.swap(failedShapeBuilds);
@ -510,6 +535,7 @@ void AvatarManager::deleteAllAvatars() {
if (avatar != _myAvatar) {
auto otherAvatar = std::static_pointer_cast<OtherAvatar>(avatar);
assert(!otherAvatar->_motionState);
assert(otherAvatar->getDetailedMotionStates().size() == 0);
}
}
}
@ -602,103 +628,135 @@ AvatarSharedPointer AvatarManager::getAvatarBySessionID(const QUuid& sessionID)
RayToAvatarIntersectionResult AvatarManager::findRayIntersection(const PickRay& ray,
const QScriptValue& avatarIdsToInclude,
const QScriptValue& avatarIdsToDiscard) {
const QScriptValue& avatarIdsToDiscard,
bool pickAgainstMesh) {
QVector<EntityItemID> avatarsToInclude = qVectorEntityItemIDFromScriptValue(avatarIdsToInclude);
QVector<EntityItemID> avatarsToDiscard = qVectorEntityItemIDFromScriptValue(avatarIdsToDiscard);
return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard);
return findRayIntersectionVector(ray, avatarsToInclude, avatarsToDiscard, pickAgainstMesh);
}
RayToAvatarIntersectionResult AvatarManager::findRayIntersectionVector(const PickRay& ray,
const QVector<EntityItemID>& avatarsToInclude,
const QVector<EntityItemID>& avatarsToDiscard) {
const QVector<EntityItemID>& avatarsToDiscard,
bool pickAgainstMesh) {
RayToAvatarIntersectionResult result;
if (QThread::currentThread() != thread()) {
BLOCKING_INVOKE_METHOD(const_cast<AvatarManager*>(this), "findRayIntersectionVector",
Q_RETURN_ARG(RayToAvatarIntersectionResult, result),
Q_ARG(const PickRay&, ray),
Q_ARG(const QVector<EntityItemID>&, avatarsToInclude),
Q_ARG(const QVector<EntityItemID>&, avatarsToDiscard));
Q_ARG(const QVector<EntityItemID>&, avatarsToDiscard),
Q_ARG(bool, pickAgainstMesh));
return result;
}
// It's better to intersect the ray against the avatar's actual mesh, but this is currently difficult to
// do, because the transformed mesh data only exists over in GPU-land. As a compromise, this code
// intersects against the avatars capsule and then against the (T-pose) mesh. The end effect is that picking
// against the avatar is sort-of right, but you likely wont be able to pick against the arms.
PROFILE_RANGE(simulation_physics, __FUNCTION__);
// TODO -- find a way to extract transformed avatar mesh data from the rendering engine.
float distance = (float)INT_MAX; // with FLT_MAX bullet rayTest does not return results
glm::vec3 rayDirection = glm::normalize(ray.direction);
std::vector<MyCharacterController::RayAvatarResult> physicsResults = _myAvatar->getCharacterController()->rayTest(glmToBullet(ray.origin), glmToBullet(rayDirection), distance, QVector<uint>());
if (physicsResults.size() > 0) {
glm::vec3 rayDirectionInv = { rayDirection.x != 0.0f ? 1.0f / rayDirection.x : INFINITY,
rayDirection.y != 0.0f ? 1.0f / rayDirection.y : INFINITY,
rayDirection.z != 0.0f ? 1.0f / rayDirection.z : INFINITY };
std::vector<SortedAvatar> sortedAvatars;
auto avatarHashCopy = getHashCopy();
for (auto avatarData : avatarHashCopy) {
auto avatar = std::static_pointer_cast<Avatar>(avatarData);
if ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatar->getID())) ||
(avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatar->getID()))) {
continue;
}
float distance = FLT_MAX;
#if 0
// if we weren't picking against the capsule, we would want to pick against the avatarBounds...
SkeletonModelPointer avatarModel = avatar->getSkeletonModel();
AABox avatarBounds = avatarModel->getRenderableMeshBound();
if (avatarBounds.contains(ray.origin)) {
distance = 0.0f;
} else {
float boundDistance = FLT_MAX;
BoxFace face;
glm::vec3 surfaceNormal;
if (avatarBounds.findRayIntersection(ray.origin, ray.direction, boundDistance, face, surfaceNormal)) {
distance = boundDistance;
}
}
#else
glm::vec3 start;
glm::vec3 end;
float radius;
avatar->getCapsule(start, end, radius);
findRayCapsuleIntersection(ray.origin, ray.direction, start, end, radius, distance);
#endif
if (distance < FLT_MAX) {
sortedAvatars.emplace_back(distance, avatar);
}
}
if (sortedAvatars.size() > 1) {
static auto comparator = [](const SortedAvatar& left, const SortedAvatar& right) { return left.first < right.first; };
std::sort(sortedAvatars.begin(), sortedAvatars.end(), comparator);
}
for (auto it = sortedAvatars.begin(); it != sortedAvatars.end(); ++it) {
const SortedAvatar& sortedAvatar = *it;
// We can exit once avatarCapsuleDistance > bestDistance
if (sortedAvatar.first > result.distance) {
break;
}
float distance = FLT_MAX;
BoxFace face;
MyCharacterController::RayAvatarResult rayAvatarResult;
AvatarPointer avatar = nullptr;
BoxFace face = BoxFace::UNKNOWN_FACE;
glm::vec3 surfaceNormal;
QVariantMap extraInfo;
SkeletonModelPointer avatarModel = sortedAvatar.second->getSkeletonModel();
if (avatarModel->findRayIntersectionAgainstSubMeshes(ray.origin, ray.direction, distance, face, surfaceNormal, extraInfo, true)) {
if (distance < result.distance) {
result.intersects = true;
result.avatarID = sortedAvatar.second->getID();
result.distance = distance;
result.face = face;
result.surfaceNormal = surfaceNormal;
result.extraInfo = extraInfo;
for (auto &hit : physicsResults) {
auto avatarID = hit._intersectWithAvatar;
if ((avatarsToInclude.size() > 0 && !avatarsToInclude.contains(avatarID)) ||
(avatarsToDiscard.size() > 0 && avatarsToDiscard.contains(avatarID))) {
continue;
}
if (_myAvatar->getSessionUUID() != avatarID) {
auto avatarMap = getHashCopy();
AvatarHash::iterator itr = avatarMap.find(avatarID);
if (itr != avatarMap.end()) {
avatar = std::static_pointer_cast<Avatar>(*itr);
}
} else {
avatar = _myAvatar;
}
if (!hit._isBound) {
rayAvatarResult = hit;
} else if (avatar) {
auto &multiSpheres = avatar->getMultiSphereShapes();
if (multiSpheres.size() > 0) {
std::vector<MyCharacterController::RayAvatarResult> boxHits;
for (size_t i = 0; i < hit._boundJoints.size(); i++) {
assert(hit._boundJoints[i] < multiSpheres.size());
auto &mSphere = multiSpheres[hit._boundJoints[i]];
if (mSphere.isValid()) {
float boundDistance = FLT_MAX;
BoxFace face;
glm::vec3 surfaceNormal;
auto &bbox = mSphere.getBoundingBox();
if (bbox.findRayIntersection(ray.origin, rayDirection, rayDirectionInv, boundDistance, face, surfaceNormal)) {
MyCharacterController::RayAvatarResult boxHit;
boxHit._distance = boundDistance;
boxHit._intersect = true;
boxHit._intersectionNormal = surfaceNormal;
boxHit._intersectionPoint = ray.origin + boundDistance * rayDirection;
boxHit._intersectWithAvatar = avatarID;
boxHit._intersectWithJoint = mSphere.getJointIndex();
boxHits.push_back(boxHit);
}
}
}
if (boxHits.size() > 0) {
if (boxHits.size() > 1) {
std::sort(boxHits.begin(), boxHits.end(), [](const MyCharacterController::RayAvatarResult& hitA,
const MyCharacterController::RayAvatarResult& hitB) {
return hitA._distance < hitB._distance;
});
}
rayAvatarResult = boxHits[0];
}
}
}
if (pickAgainstMesh) {
glm::vec3 localRayOrigin = avatar->worldToJointPoint(ray.origin, rayAvatarResult._intersectWithJoint);
glm::vec3 localRayPoint = avatar->worldToJointPoint(ray.origin + rayDirection, rayAvatarResult._intersectWithJoint);
auto avatarOrientation = avatar->getWorldOrientation();
auto avatarPosition = avatar->getWorldPosition();
auto jointOrientation = avatarOrientation * avatar->getAbsoluteDefaultJointRotationInObjectFrame(rayAvatarResult._intersectWithJoint);
auto jointPosition = avatarPosition + (avatarOrientation * avatar->getAbsoluteDefaultJointTranslationInObjectFrame(rayAvatarResult._intersectWithJoint));
auto defaultFrameRayOrigin = jointPosition + jointOrientation * localRayOrigin;
auto defaultFrameRayPoint = jointPosition + jointOrientation * localRayPoint;
auto defaultFrameRayDirection = defaultFrameRayPoint - defaultFrameRayOrigin;
if (avatar->getSkeletonModel()->findRayIntersectionAgainstSubMeshes(defaultFrameRayOrigin, defaultFrameRayDirection, distance, face, surfaceNormal, extraInfo, true, false)) {
auto newDistance = glm::length(vec3FromVariant(extraInfo["worldIntersectionPoint"]) - defaultFrameRayOrigin);
rayAvatarResult._distance = newDistance;
rayAvatarResult._intersectionPoint = ray.origin + newDistance * rayDirection;
rayAvatarResult._intersectionNormal = surfaceNormal;
extraInfo["worldIntersectionPoint"] = vec3toVariant(rayAvatarResult._intersectionPoint);
break;
}
} else if (rayAvatarResult._intersect){
break;
}
}
if (rayAvatarResult._intersect) {
result.intersects = true;
result.avatarID = rayAvatarResult._intersectWithAvatar;
result.distance = rayAvatarResult._distance;
result.surfaceNormal = rayAvatarResult._intersectionNormal;
result.jointIndex = rayAvatarResult._intersectWithJoint;
result.intersection = ray.origin + rayAvatarResult._distance * rayDirection;
result.extraInfo = extraInfo;
result.face = face;
}
}
if (result.intersects) {
result.intersection = ray.origin + ray.direction * result.distance;
}
return result;
}

9
interface/src/avatar/AvatarManager.h
View file

@ -29,6 +29,7 @@
#include <EntitySimulation.h> // for SetOfEntities
#include "AvatarMotionState.h"
#include "DetailedMotionState.h"
#include "MyAvatar.h"
#include "OtherAvatar.h"
@ -136,21 +137,25 @@ public:
* @param {PickRay} ray
* @param {Uuid[]} [avatarsToInclude=[]]
* @param {Uuid[]} [avatarsToDiscard=[]]
* @param {boolean} pickAgainstMesh
* @returns {RayToAvatarIntersectionResult}
*/
Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersection(const PickRay& ray,
const QScriptValue& avatarIdsToInclude = QScriptValue(),
const QScriptValue& avatarIdsToDiscard = QScriptValue());
const QScriptValue& avatarIdsToDiscard = QScriptValue(),
bool pickAgainstMesh = true);
/**jsdoc
* @function AvatarManager.findRayIntersectionVector
* @param {PickRay} ray
* @param {Uuid[]} avatarsToInclude
* @param {Uuid[]} avatarsToDiscard
* @param {boolean} pickAgainstMesh
* @returns {RayToAvatarIntersectionResult}
*/
Q_INVOKABLE RayToAvatarIntersectionResult findRayIntersectionVector(const PickRay& ray,
const QVector<EntityItemID>& avatarsToInclude,
const QVector<EntityItemID>& avatarsToDiscard);
const QVector<EntityItemID>& avatarsToDiscard,
bool pickAgainstMesh);
/**jsdoc
* @function AvatarManager.findParabolaIntersectionVector

188
interface/src/avatar/DetailedMotionState.cpp Normal file
View file

@ -0,0 +1,188 @@
//
// DetailedMotionState.cpp
// interface/src/avatar/
//
// Created by Luis Cuenca 1/11/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 "DetailedMotionState.h"
#include <PhysicsCollisionGroups.h>
#include <PhysicsEngine.h>
#include <PhysicsHelpers.h>
#include "MyAvatar.h"
DetailedMotionState::DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex) :
ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) {
assert(_avatar);
if (!_avatar->isMyAvatar()) {
_otherAvatar = std::static_pointer_cast<OtherAvatar>(_avatar);
}
_type = MOTIONSTATE_TYPE_DETAILED;
}
void DetailedMotionState::handleEasyChanges(uint32_t& flags) {
ObjectMotionState::handleEasyChanges(flags);
if (flags & Simulation::DIRTY_PHYSICS_ACTIVATION && !_body->isActive()) {
_body->activate();
}
}
bool DetailedMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) {
return ObjectMotionState::handleHardAndEasyChanges(flags, engine);
}
DetailedMotionState::~DetailedMotionState() {
assert(_avatar);
_avatar = nullptr;
}
// virtual
uint32_t DetailedMotionState::getIncomingDirtyFlags() {
return _body ? _dirtyFlags : 0;
}
void DetailedMotionState::clearIncomingDirtyFlags() {
if (_body) {
_dirtyFlags = 0;
}
}
PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const {
// TODO?: support non-DYNAMIC motion for avatars? (e.g. when sitting)
return MOTION_TYPE_KINEMATIC;
}
// virtual and protected
const btCollisionShape* DetailedMotionState::computeNewShape() {
btCollisionShape* shape = nullptr;
if (!_avatar->isMyAvatar()) {
if (_otherAvatar != nullptr) {
shape = _otherAvatar->createCollisionShape(_jointIndex, _isBound, _boundJoints);
}
} else {
std::shared_ptr<MyAvatar> myAvatar = std::static_pointer_cast<MyAvatar>(_avatar);
if (myAvatar) {
shape = myAvatar->getCharacterController()->createDetailedCollisionShapeForJoint(_jointIndex);
}
}
return shape;
}
// virtual
bool DetailedMotionState::isMoving() const {
return false;
}
// virtual
void DetailedMotionState::getWorldTransform(btTransform& worldTrans) const {
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
worldTrans.setRotation(glmToBullet(getObjectRotation()));
}
// virtual
void DetailedMotionState::setWorldTransform(const btTransform& worldTrans) {
_body->setWorldTransform(worldTrans);
}
// These pure virtual methods must be implemented for each MotionState type
// and make it possible to implement more complicated methods in this base class.
// virtual
float DetailedMotionState::getObjectRestitution() const {
return 0.5f;
}
// virtual
float DetailedMotionState::getObjectFriction() const {
return 0.5f;
}
// virtual
float DetailedMotionState::getObjectLinearDamping() const {
return 0.5f;
}
// virtual
float DetailedMotionState::getObjectAngularDamping() const {
return 0.5f;
}
// virtual
glm::vec3 DetailedMotionState::getObjectPosition() const {
if (_otherAvatar != nullptr) {
auto bodyLOD = _otherAvatar->getBodyLOD();
if (bodyLOD == OtherAvatar::BodyLOD::Sphere) {
return _avatar->getFitBounds().calcCenter();
}
}
return _avatar->getJointPosition(_jointIndex);
}
// virtual
glm::quat DetailedMotionState::getObjectRotation() const {
return _avatar->getWorldOrientation() * _avatar->getAbsoluteJointRotationInObjectFrame(_jointIndex);
}
// virtual
glm::vec3 DetailedMotionState::getObjectLinearVelocity() const {
return glm::vec3(0.0f);
}
// virtual
glm::vec3 DetailedMotionState::getObjectAngularVelocity() const {
return glm::vec3(0.0f);
}
// virtual
glm::vec3 DetailedMotionState::getObjectGravity() const {
return glm::vec3(0.0f);
}
// virtual
const QUuid DetailedMotionState::getObjectID() const {
return _avatar->getSessionUUID();
}
QString DetailedMotionState::getName() const {
return _avatar->getName() + "_" + _jointIndex;
}
// virtual
QUuid DetailedMotionState::getSimulatorID() const {
return _avatar->getSessionUUID();
}
// virtual
void DetailedMotionState::computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const {
group = BULLET_COLLISION_GROUP_DETAILED_AVATAR;
mask = Physics::getDefaultCollisionMask(group);
}
// virtual
float DetailedMotionState::getMass() const {
return 0.0f;
}
void DetailedMotionState::setRigidBody(btRigidBody* body) {
ObjectMotionState::setRigidBody(body);
if (_body) {
// remove angular dynamics from this body
_body->setAngularFactor(0.0f);
}
}
void DetailedMotionState::setShape(const btCollisionShape* shape) {
ObjectMotionState::setShape(shape);
}
void DetailedMotionState::forceActive() {
if (_body && !_body->isActive()) {
_body->setActivationState(ACTIVE_TAG);
}
}

100
interface/src/avatar/DetailedMotionState.h Normal file
View file

@ -0,0 +1,100 @@
//
// DetailedMotionState.h
// interface/src/avatar/
//
// Created by Luis Cuenca 1/11/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
//
#ifndef hifi_DetailedMotionState_h
#define hifi_DetailedMotionState_h
#include <QSet>
#include <ObjectMotionState.h>
#include <BulletUtil.h>
#include "OtherAvatar.h"
class DetailedMotionState : public ObjectMotionState {
public:
DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex);
virtual void handleEasyChanges(uint32_t& flags) override;
virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
virtual PhysicsMotionType getMotionType() const override { return _motionType; }
virtual uint32_t getIncomingDirtyFlags() override;
virtual void clearIncomingDirtyFlags() override;
virtual PhysicsMotionType computePhysicsMotionType() const override;
virtual bool isMoving() const override;
// this relays incoming position/rotation to the RigidBody
virtual void getWorldTransform(btTransform& worldTrans) const override;
// this relays outgoing position/rotation to the EntityItem
virtual void setWorldTransform(const btTransform& worldTrans) override;
// These pure virtual methods must be implemented for each MotionState type
// and make it possible to implement more complicated methods in this base class.
// pure virtual overrides from ObjectMotionState
virtual float getObjectRestitution() const override;
virtual float getObjectFriction() const override;
virtual float getObjectLinearDamping() const override;
virtual float getObjectAngularDamping() const override;
virtual glm::vec3 getObjectPosition() const override;
virtual glm::quat getObjectRotation() const override;
virtual glm::vec3 getObjectLinearVelocity() const override;
virtual glm::vec3 getObjectAngularVelocity() const override;
virtual glm::vec3 getObjectGravity() const override;
virtual const QUuid getObjectID() const override;
virtual QString getName() const override;
virtual QUuid getSimulatorID() const override;
void addDirtyFlags(uint32_t flags) { _dirtyFlags |= flags; }
virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override;
virtual float getMass() const override;
void forceActive();
QUuid getAvatarID() const { return _avatar->getID(); }
int getJointIndex() const { return _jointIndex; }
void setIsBound(bool isBound, std::vector<int> boundJoints) { _isBound = isBound; _boundJoints = boundJoints; }
bool getIsBound(std::vector<int>& boundJoints) const { boundJoints = _boundJoints; return _isBound; }
friend class AvatarManager;
friend class Avatar;
protected:
void setRigidBody(btRigidBody* body) override;
void setShape(const btCollisionShape* shape) override;
// the dtor had been made protected to force the compiler to verify that it is only
// ever called by the Avatar class dtor.
~DetailedMotionState();
virtual bool isReadyToComputeShape() const override { return true; }
virtual const btCollisionShape* computeNewShape() override;
AvatarPointer _avatar;
float _diameter { 0.0f };
uint32_t _dirtyFlags;
int _jointIndex { -1 };
OtherAvatarPointer _otherAvatar { nullptr };
bool _isBound { false };
std::vector<int> _boundJoints;
};
#endif // hifi_DetailedMotionState_h

93
interface/src/avatar/MyAvatar.cpp
View file

@ -134,7 +134,7 @@ MyAvatar::MyAvatar(QThread* thread) :
_scriptedMotorFrame(SCRIPTED_MOTOR_CAMERA_FRAME),
_scriptedMotorMode(SCRIPTED_MOTOR_SIMPLE_MODE),
_motionBehaviors(AVATAR_MOTION_DEFAULTS),
_characterController(this),
_characterController(std::shared_ptr<MyAvatar>(this)),
_eyeContactTarget(LEFT_EYE),
_realWorldFieldOfView("realWorldFieldOfView",
DEFAULT_REAL_WORLD_FIELD_OF_VIEW_DEGREES),
@ -1171,77 +1171,6 @@ controller::Pose MyAvatar::getRightHandTipPose() const {
return pose;
}
glm::vec3 MyAvatar::worldToJointPoint(const glm::vec3& position, const int jointIndex) const {
glm::vec3 jointPos = getWorldPosition();//default value if no or invalid joint specified
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if (jointIndex != -1) {
if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) {
_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot);
} else {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
}
glm::vec3 modelOffset = position - jointPos;
glm::vec3 jointSpacePosition = glm::inverse(jointRot) * modelOffset;
return jointSpacePosition;
}
glm::vec3 MyAvatar::worldToJointDirection(const glm::vec3& worldDir, const int jointIndex) const {
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
glm::vec3 jointSpaceDir = glm::inverse(jointRot) * worldDir;
return jointSpaceDir;
}
glm::quat MyAvatar::worldToJointRotation(const glm::quat& worldRot, const int jointIndex) const {
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
glm::quat jointSpaceRot = glm::inverse(jointRot) * worldRot;
return jointSpaceRot;
}
glm::vec3 MyAvatar::jointToWorldPoint(const glm::vec3& jointSpacePos, const int jointIndex) const {
glm::vec3 jointPos = getWorldPosition();//default value if no or invalid joint specified
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if (jointIndex != -1) {
if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) {
_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot);
} else {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
}
glm::vec3 worldOffset = jointRot * jointSpacePos;
glm::vec3 worldPos = jointPos + worldOffset;
return worldPos;
}
glm::vec3 MyAvatar::jointToWorldDirection(const glm::vec3& jointSpaceDir, const int jointIndex) const {
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
glm::vec3 worldDir = jointRot * jointSpaceDir;
return worldDir;
}
glm::quat MyAvatar::jointToWorldRotation(const glm::quat& jointSpaceRot, const int jointIndex) const {
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
glm::quat worldRot = jointRot * jointSpaceRot;
return worldRot;
}
// virtual
void MyAvatar::render(RenderArgs* renderArgs) {
// don't render if we've been asked to disable local rendering
@ -3114,16 +3043,15 @@ void MyAvatar::postUpdate(float deltaTime, const render::ScenePointer& scene) {
if (_skeletonModel && _skeletonModel->isLoaded()) {
const Rig& rig = _skeletonModel->getRig();
const HFMModel& hfmModel = _skeletonModel->getHFMModel();
for (int i = 0; i < rig.getJointStateCount(); i++) {
AnimPose jointPose;
rig.getAbsoluteJointPoseInRigFrame(i, jointPose);
const HFMJointShapeInfo& shapeInfo = hfmModel.joints[i].shapeInfo;
const AnimPose pose = rigToWorldPose * jointPose;
for (size_t j = 0; j < shapeInfo.debugLines.size() / 2; j++) {
glm::vec3 pointA = pose.xformPoint(shapeInfo.debugLines[2 * j]);
glm::vec3 pointB = pose.xformPoint(shapeInfo.debugLines[2 * j + 1]);
DebugDraw::getInstance().drawRay(pointA, pointB, DEBUG_COLORS[i % NUM_DEBUG_COLORS]);
int jointCount = rig.getJointStateCount();
if (jointCount == (int)_multiSphereShapes.size()) {
for (int i = 0; i < jointCount; i++) {
AnimPose jointPose;
rig.getAbsoluteJointPoseInRigFrame(i, jointPose);
const AnimPose pose = rigToWorldPose * jointPose;
auto &multiSphere = _multiSphereShapes[i];
auto debugLines = multiSphere.getDebugLines();
DebugDraw::getInstance().drawRays(debugLines, DEBUG_COLORS[i % NUM_DEBUG_COLORS], pose.trans(), pose.rot());
}
}
}
@ -5383,3 +5311,4 @@ void MyAvatar::releaseGrab(const QUuid& grabID) {
_clientTraitsHandler->markInstancedTraitDeleted(AvatarTraits::Grab, grabID);
}
}

51
interface/src/avatar/MyAvatar.h
View file

@ -37,6 +37,7 @@
class AvatarActionHold;
class ModelItemID;
class MyHead;
class DetailedMotionState;
enum eyeContactTarget {
LEFT_EYE,
@ -802,56 +803,6 @@ public:
*/
Q_INVOKABLE controller::Pose getRightHandTipPose() const;
// world-space to avatar-space rigconversion functions
/**jsdoc
* @function MyAvatar.worldToJointPoint
* @param {Vec3} position
* @param {number} [jointIndex=-1]
* @returns {Vec3}
*/
Q_INVOKABLE glm::vec3 worldToJointPoint(const glm::vec3& position, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.worldToJointDirection
* @param {Vec3} direction
* @param {number} [jointIndex=-1]
* @returns {Vec3}
*/
Q_INVOKABLE glm::vec3 worldToJointDirection(const glm::vec3& direction, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.worldToJointRotation
* @param {Quat} rotation
* @param {number} [jointIndex=-1]
* @returns {Quat}
*/
Q_INVOKABLE glm::quat worldToJointRotation(const glm::quat& rotation, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.jointToWorldPoint
* @param {vec3} position
* @param {number} [jointIndex=-1]
* @returns {Vec3}
*/
Q_INVOKABLE glm::vec3 jointToWorldPoint(const glm::vec3& position, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.jointToWorldDirection
* @param {Vec3} direction
* @param {number} [jointIndex=-1]
* @returns {Vec3}
*/
Q_INVOKABLE glm::vec3 jointToWorldDirection(const glm::vec3& direction, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.jointToWorldRotation
* @param {Quat} rotation
* @param {number} [jointIndex=-1]
* @returns {Quat}
*/
Q_INVOKABLE glm::quat jointToWorldRotation(const glm::quat& rotation, const int jointIndex = -1) const;
AvatarWeakPointer getLookAtTargetAvatar() const { return _lookAtTargetAvatar; }
void updateLookAtTargetAvatar();
void computeMyLookAtTarget(const AvatarHash& hash);

123
interface/src/avatar/MyCharacterController.cpp
View file

@ -12,8 +12,10 @@
#include "MyCharacterController.h"
#include <BulletUtil.h>
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
#include "MyAvatar.h"
#include "DetailedMotionState.h"
// TODO: make avatars stand on steep slope
// TODO: make avatars not snag on low ceilings
@ -24,7 +26,7 @@ void MyCharacterController::RayShotgunResult::reset() {
walkable = true;
}
MyCharacterController::MyCharacterController(MyAvatar* avatar) {
MyCharacterController::MyCharacterController(std::shared_ptr<MyAvatar> avatar) {
assert(avatar);
_avatar = avatar;
@ -44,7 +46,6 @@ void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) {
void MyCharacterController::updateShapeIfNecessary() {
if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
_pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE;
if (_radius > 0.0f) {
// create RigidBody if it doesn't exist
if (!_rigidBody) {
@ -375,3 +376,121 @@ void MyCharacterController::updateMassProperties() {
_rigidBody->setMassProps(mass, inertia);
}
btCollisionShape* MyCharacterController::createDetailedCollisionShapeForJoint(int jointIndex) {
ShapeInfo shapeInfo;
_avatar->computeDetailedShapeInfo(shapeInfo, jointIndex);
if (shapeInfo.getType() != SHAPE_TYPE_NONE) {
btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
if (shape) {
shape->setMargin(0.001f);
}
return shape;
}
return nullptr;
}
DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(int jointIndex) {
auto shape = createDetailedCollisionShapeForJoint(jointIndex);
if (shape) {
DetailedMotionState* motionState = new DetailedMotionState(_avatar, shape, jointIndex);
motionState->setMass(_avatar->computeMass());
return motionState;
}
return nullptr;
}
void MyCharacterController::clearDetailedMotionStates() {
_pendingFlags |= PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
// We make sure we don't add them again
_pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;
}
void MyCharacterController::resetDetailedMotionStates() {
_detailedMotionStates.clear();
}
void MyCharacterController::buildPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
_detailedMotionStates[i]->forceActive();
}
if (_pendingFlags & PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION) {
_pendingFlags &= ~PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
transaction.objectsToRemove.push_back(_detailedMotionStates[i]);
}
_detailedMotionStates.clear();
}
if (_pendingFlags & PENDING_FLAG_ADD_DETAILED_TO_SIMULATION) {
_pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;
for (int i = 0; i < _avatar->getJointCount(); i++) {
auto dMotionState = createDetailedMotionStateForJoint(i);
if (dMotionState) {
_detailedMotionStates.push_back(dMotionState);
transaction.objectsToAdd.push_back(dMotionState);
}
}
}
}
void MyCharacterController::handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
// things on objectsToRemove are ready for delete
for (auto object : transaction.objectsToRemove) {
delete object;
}
transaction.clear();
}
class DetailedRayResultCallback : public btCollisionWorld::AllHitsRayResultCallback {
public:
DetailedRayResultCallback()
: btCollisionWorld::AllHitsRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) {
// the RayResultCallback's group and mask must match MY_AVATAR
m_collisionFilterGroup = BULLET_COLLISION_GROUP_DETAILED_RAY;
m_collisionFilterMask = BULLET_COLLISION_MASK_DETAILED_RAY;
}
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) override {
return AllHitsRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
}
};
std::vector<MyCharacterController::RayAvatarResult> MyCharacterController::rayTest(const btVector3& origin, const btVector3& direction,
const btScalar& length, const QVector<uint>& jointsToExclude) const {
std::vector<RayAvatarResult> foundAvatars;
if (_dynamicsWorld) {
btVector3 end = origin + length * direction;
DetailedRayResultCallback rayCallback = DetailedRayResultCallback();
rayCallback.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;
rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest;
_dynamicsWorld->rayTest(origin, end, rayCallback);
if (rayCallback.m_hitFractions.size() > 0) {
foundAvatars.reserve(rayCallback.m_hitFractions.size());
for (int i = 0; i < rayCallback.m_hitFractions.size(); i++) {
auto object = rayCallback.m_collisionObjects[i];
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(object->getUserPointer());
if (motionState && motionState->getType() == MOTIONSTATE_TYPE_DETAILED) {
DetailedMotionState* detailedMotionState = dynamic_cast<DetailedMotionState*>(motionState);
MyCharacterController::RayAvatarResult result;
result._intersect = true;
result._intersectWithAvatar = detailedMotionState->getAvatarID();
result._intersectionPoint = bulletToGLM(rayCallback.m_hitPointWorld[i]);
result._intersectionNormal = bulletToGLM(rayCallback.m_hitNormalWorld[i]);
result._distance = length * rayCallback.m_hitFractions[i];
result._intersectWithJoint = detailedMotionState->getJointIndex();
result._isBound = detailedMotionState->getIsBound(result._boundJoints);
btVector3 center;
btScalar radius;
detailedMotionState->getShape()->getBoundingSphere(center, radius);
result._maxDistance = (float)radius;
foundAvatars.push_back(result);
}
}
std::sort(foundAvatars.begin(), foundAvatars.end(), [](const RayAvatarResult& resultA, const RayAvatarResult& resultB) {
return resultA._distance < resultB._distance;
});
}
}
return foundAvatars;
}

33
interface/src/avatar/MyCharacterController.h Executable file → Normal file
View file

@ -15,13 +15,15 @@
#include <CharacterController.h>
//#include <SharedUtil.h>
#include <PhysicsEngine.h>
class btCollisionShape;
class MyAvatar;
class DetailedMotionState;
class MyCharacterController : public CharacterController {
public:
explicit MyCharacterController(MyAvatar* avatar);
explicit MyCharacterController(std::shared_ptr<MyAvatar> avatar);
~MyCharacterController ();
void setDynamicsWorld(btDynamicsWorld* world) override;
@ -42,6 +44,31 @@ public:
void setDensity(btScalar density) { _density = density; }
btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex);
DetailedMotionState* createDetailedMotionStateForJoint(int jointIndex);
std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
void clearDetailedMotionStates();
void resetDetailedMotionStates();
void buildPhysicsTransaction(PhysicsEngine::Transaction& transaction);
void handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction);
struct RayAvatarResult {
bool _intersect { false };
bool _isBound { false };
QUuid _intersectWithAvatar;
int _intersectWithJoint { -1 };
float _distance { 0.0f };
float _maxDistance { 0.0f };
QVariantMap _extraInfo;
glm::vec3 _intersectionPoint;
glm::vec3 _intersectionNormal;
std::vector<int> _boundJoints;
};
std::vector<RayAvatarResult> rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length,
const QVector<uint>& jointsToExclude) const;
int32_t computeCollisionMask() const override;
void handleChangedCollisionMask() override;
@ -56,12 +83,14 @@ private:
btConvexHullShape* computeShape() const;
protected:
MyAvatar* _avatar { nullptr };
std::shared_ptr<MyAvatar> _avatar { nullptr };
// shotgun scan data
btAlignedObjectArray<btVector3> _topPoints;
btAlignedObjectArray<btVector3> _bottomPoints;
btScalar _density { 1.0f };
std::vector<DetailedMotionState*> _detailedMotionStates;
};
#endif // hifi_MyCharacterController_h

150
interface/src/avatar/OtherAvatar.cpp
View file

@ -15,6 +15,7 @@
#include "Application.h"
#include "AvatarMotionState.h"
#include "DetailedMotionState.h"
const float DISPLAYNAME_FADE_TIME = 0.5f;
const float DISPLAYNAME_FADE_FACTOR = pow(0.01f, 1.0f / DISPLAYNAME_FADE_TIME);
@ -110,29 +111,155 @@ void OtherAvatar::updateSpaceProxy(workload::Transaction& transaction) const {
int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) {
int32_t bytesRead = Avatar::parseDataFromBuffer(buffer);
for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
_detailedMotionStates[i]->forceActive();
}
if (_moving && _motionState) {
_motionState->addDirtyFlags(Simulation::DIRTY_POSITION);
}
return bytesRead;
}
btCollisionShape* OtherAvatar::createCollisionShape(int jointIndex, bool& isBound, std::vector<int>& boundJoints) {
ShapeInfo shapeInfo;
isBound = false;
QString jointName = "";
if (jointIndex > -1 && jointIndex < (int)_multiSphereShapes.size()) {
jointName = _multiSphereShapes[jointIndex].getJointName();
}
switch (_bodyLOD) {
case BodyLOD::Sphere:
shapeInfo.setSphere(0.5f * getFitBounds().getDimensions().y);
boundJoints.clear();
for (auto &spheres : _multiSphereShapes) {
if (spheres.isValid()) {
boundJoints.push_back(spheres.getJointIndex());
}
}
isBound = true;
break;
case BodyLOD::MultiSphereLow:
if (jointName.contains("RightHand", Qt::CaseInsensitive) || jointName.contains("LeftHand", Qt::CaseInsensitive)) {
if (jointName.size() <= QString("RightHand").size()) {
AABox handBound;
for (auto &spheres : _multiSphereShapes) {
if (spheres.isValid() && spheres.getJointName().contains(jointName, Qt::CaseInsensitive)) {
boundJoints.push_back(spheres.getJointIndex());
handBound += spheres.getBoundingBox();
}
}
shapeInfo.setSphere(0.5f * handBound.getLargestDimension());
glm::vec3 jointPosition;
glm::quat jointRotation;
_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPosition);
_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRotation);
glm::vec3 positionOffset = glm::inverse(jointRotation) * (handBound.calcCenter() - jointPosition);
shapeInfo.setOffset(positionOffset);
isBound = true;
}
break;
}
case BodyLOD::MultiSphereHigh:
computeDetailedShapeInfo(shapeInfo, jointIndex);
break;
default:
break;
}
if (shapeInfo.getType() != SHAPE_TYPE_NONE) {
auto shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
if (shape) {
shape->setMargin(0.001f);
}
return shape;
}
return nullptr;
}
DetailedMotionState* OtherAvatar::createMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex) {
bool isBound = false;
std::vector<int> boundJoints;
btCollisionShape* shape = createCollisionShape(jointIndex, isBound, boundJoints);
if (shape) {
DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex);
motionState->setMass(computeMass());
motionState->setIsBound(isBound, boundJoints);
return motionState;
}
return nullptr;
}
void OtherAvatar::resetDetailedMotionStates() {
for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
_detailedMotionStates[i] = nullptr;
}
_detailedMotionStates.clear();
}
void OtherAvatar::setWorkloadRegion(uint8_t region) {
_workloadRegion = region;
QString printRegion = "";
if (region == workload::Region::R1) {
printRegion = "R1";
} else if (region == workload::Region::R2) {
printRegion = "R2";
} else if (region == workload::Region::R3) {
printRegion = "R3";
} else {
printRegion = "invalid";
}
qCDebug(avatars) << "Setting workload region to " << printRegion;
computeShapeLOD();
}
void OtherAvatar::computeShapeLOD() {
// auto newBodyLOD = _workloadRegion < workload::Region::R3 ? BodyLOD::MultiSphereShapes : BodyLOD::CapsuleShape;
// auto newBodyLOD = BodyLOD::CapsuleShape;
BodyLOD newLOD;
switch (_workloadRegion) {
case workload::Region::R1:
newLOD = BodyLOD::MultiSphereHigh;
break;
case workload::Region::R2:
newLOD = BodyLOD::MultiSphereLow;
break;
case workload::Region::UNKNOWN:
case workload::Region::INVALID:
case workload::Region::R3:
default:
newLOD = BodyLOD::Sphere;
break;
}
if (newLOD != _bodyLOD) {
_bodyLOD = newLOD;
if (isInPhysicsSimulation()) {
qCDebug(avatars) << "Changing to body LOD " << newLOD;
_needsReinsertion = true;
}
}
}
bool OtherAvatar::isInPhysicsSimulation() const {
return _motionState != nullptr && _detailedMotionStates.size() > 0;
}
bool OtherAvatar::shouldBeInPhysicsSimulation() const {
return (_workloadRegion < workload::Region::R3 && !isDead());
return !isDead() && !(isInPhysicsSimulation() && _needsReinsertion);
}
bool OtherAvatar::needsPhysicsUpdate() const {
constexpr uint32_t FLAGS_OF_INTEREST = Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS | Simulation::DIRTY_POSITION | Simulation::DIRTY_COLLISION_GROUP;
return (_motionState && (bool)(_motionState->getIncomingDirtyFlags() & FLAGS_OF_INTEREST));
return (_needsReinsertion || (_motionState && (bool)(_motionState->getIncomingDirtyFlags() & FLAGS_OF_INTEREST)));
}
void OtherAvatar::rebuildCollisionShape() {
if (_motionState) {
_motionState->addDirtyFlags(Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS);
}
for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
if (_detailedMotionStates[i]) {
_detailedMotionStates[i]->addDirtyFlags(Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS);
}
}
}
void OtherAvatar::setCollisionWithOtherAvatarsFlags() {
@ -141,6 +268,25 @@ void OtherAvatar::setCollisionWithOtherAvatarsFlags() {
}
}
void OtherAvatar::createDetailedMotionStates(const std::shared_ptr<OtherAvatar>& avatar) {
auto& detailedMotionStates = getDetailedMotionStates();
assert(detailedMotionStates.empty());
if (_bodyLOD == BodyLOD::Sphere) {
auto dMotionState = createMotionState(avatar, -1);
if (dMotionState) {
detailedMotionStates.push_back(dMotionState);
}
} else {
for (int i = 0; i < getJointCount(); i++) {
auto dMotionState = createMotionState(avatar, i);
if (dMotionState) {
detailedMotionStates.push_back(dMotionState);
}
}
}
_needsReinsertion = false;
}
void OtherAvatar::simulate(float deltaTime, bool inView) {
PROFILE_RANGE(simulation, "simulate");

22
interface/src/avatar/OtherAvatar.h Executable file → Normal file
View file

@ -21,12 +21,19 @@
class AvatarManager;
class AvatarMotionState;
class DetailedMotionState;
class OtherAvatar : public Avatar {
public:
explicit OtherAvatar(QThread* thread);
virtual ~OtherAvatar();
enum BodyLOD {
Sphere = 0,
MultiSphereLow, // No finger joints
MultiSphereHigh // All joints
};
virtual void instantiableAvatar() override { };
virtual void createOrb() override;
virtual void indicateLoadingStatus(LoadingStatus loadingStatus) override;
@ -39,13 +46,22 @@ public:
int parseDataFromBuffer(const QByteArray& buffer) override;
bool isInPhysicsSimulation() const { return _motionState != nullptr; }
bool isInPhysicsSimulation() const;
void rebuildCollisionShape() override;
void setWorkloadRegion(uint8_t region);
bool shouldBeInPhysicsSimulation() const;
bool needsPhysicsUpdate() const;
btCollisionShape* createCollisionShape(int jointIndex, bool& isBound, std::vector<int>& boundJoints);
DetailedMotionState* createMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex);
void createDetailedMotionStates(const std::shared_ptr<OtherAvatar>& avatar);
std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
void resetDetailedMotionStates();
BodyLOD getBodyLOD() { return _bodyLOD; }
void computeShapeLOD();
void updateCollisionGroup(bool myAvatarCollide);
bool getCollideWithOtherAvatars() const { return _collideWithOtherAvatars; }
void setCollisionWithOtherAvatarsFlags() override;
@ -64,10 +80,14 @@ protected:
std::shared_ptr<Sphere3DOverlay> _otherAvatarOrbMeshPlaceholder { nullptr };
OverlayID _otherAvatarOrbMeshPlaceholderID { UNKNOWN_OVERLAY_ID };
AvatarMotionState* _motionState { nullptr };
std::vector<DetailedMotionState*> _detailedMotionStates;
int32_t _spaceIndex { -1 };
uint8_t _workloadRegion { workload::Region::INVALID };
BodyLOD _bodyLOD { BodyLOD::Sphere };
bool _needsReinsertion { false };
};
using OtherAvatarPointer = std::shared_ptr<OtherAvatar>;
using AvatarPointer = std::shared_ptr<Avatar>;
#endif // hifi_OtherAvatar_h

2
interface/src/raypick/RayPick.cpp
View file

@ -56,7 +56,7 @@ PickResultPointer RayPick::getOverlayIntersection(const PickRay& pick) {
}
PickResultPointer RayPick::getAvatarIntersection(const PickRay& pick) {
RayToAvatarIntersectionResult avatarRes = DependencyManager::get<AvatarManager>()->findRayIntersectionVector(pick, getIncludeItemsAs<EntityItemID>(), getIgnoreItemsAs<EntityItemID>());
RayToAvatarIntersectionResult avatarRes = DependencyManager::get<AvatarManager>()->findRayIntersectionVector(pick, getIncludeItemsAs<EntityItemID>(), getIgnoreItemsAs<EntityItemID>(), true);
if (avatarRes.intersects) {
return std::make_shared<RayPickResult>(IntersectionType::AVATAR, avatarRes.avatarID, avatarRes.distance, avatarRes.intersection, pick, avatarRes.surfaceNormal, avatarRes.extraInfo);
} else {

136
libraries/avatars-renderer/src/avatars-renderer/Avatar.cpp
View file

@ -298,7 +298,9 @@ void Avatar::setTargetScale(float targetScale) {
_scaleChanged = usecTimestampNow();
_avatarScaleChanged = _scaleChanged;
_isAnimatingScale = true;
for (auto& sphere : _multiSphereShapes) {
sphere.setScale(_targetScale);
}
emit targetScaleChanged(targetScale);
}
}
@ -722,6 +724,7 @@ void Avatar::postUpdate(float deltaTime, const render::ScenePointer& scene) {
}
fixupModelsInScene(scene);
updateFitBoundingBox();
}
void Avatar::render(RenderArgs* renderArgs) {
@ -1288,6 +1291,79 @@ glm::vec3 Avatar::getAbsoluteJointScaleInObjectFrame(int index) const {
}
}
glm::vec3 Avatar::worldToJointPoint(const glm::vec3& position, const int jointIndex) const {
glm::vec3 jointPos = getWorldPosition();//default value if no or invalid joint specified
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if (jointIndex != -1) {
if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) {
_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot);
} else {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
}
glm::vec3 modelOffset = position - jointPos;
glm::vec3 jointSpacePosition = glm::inverse(jointRot) * modelOffset;
return jointSpacePosition;
}
glm::vec3 Avatar::worldToJointDirection(const glm::vec3& worldDir, const int jointIndex) const {
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
glm::vec3 jointSpaceDir = glm::inverse(jointRot) * worldDir;
return jointSpaceDir;
}
glm::quat Avatar::worldToJointRotation(const glm::quat& worldRot, const int jointIndex) const {
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
glm::quat jointSpaceRot = glm::inverse(jointRot) * worldRot;
return jointSpaceRot;
}
glm::vec3 Avatar::jointToWorldPoint(const glm::vec3& jointSpacePos, const int jointIndex) const {
glm::vec3 jointPos = getWorldPosition();//default value if no or invalid joint specified
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if (jointIndex != -1) {
if (_skeletonModel->getJointPositionInWorldFrame(jointIndex, jointPos)) {
_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot);
} else {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
}
glm::vec3 worldOffset = jointRot * jointSpacePos;
glm::vec3 worldPos = jointPos + worldOffset;
return worldPos;
}
glm::vec3 Avatar::jointToWorldDirection(const glm::vec3& jointSpaceDir, const int jointIndex) const {
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
glm::vec3 worldDir = jointRot * jointSpaceDir;
return worldDir;
}
glm::quat Avatar::jointToWorldRotation(const glm::quat& jointSpaceRot, const int jointIndex) const {
glm::quat jointRot = getWorldOrientation();//default value if no or invalid joint specified
if ((jointIndex != -1) && (!_skeletonModel->getJointRotationInWorldFrame(jointIndex, jointRot))) {
qWarning() << "Invalid joint index specified: " << jointIndex;
}
glm::quat worldRot = jointRot * jointSpaceRot;
return worldRot;
}
void Avatar::invalidateJointIndicesCache() const {
QWriteLocker writeLock(&_modelJointIndicesCacheLock);
_modelJointsCached = false;
@ -1425,6 +1501,7 @@ void Avatar::setModelURLFinished(bool success) {
// rig is ready
void Avatar::rigReady() {
buildUnscaledEyeHeightCache();
computeMultiSphereShapes();
}
// rig has been reset.
@ -1432,6 +1509,48 @@ void Avatar::rigReset() {
clearUnscaledEyeHeightCache();
}
void Avatar::computeMultiSphereShapes() {
const Rig& rig = getSkeletonModel()->getRig();
glm::vec3 scale = extractScale(rig.getGeometryOffsetPose());
const HFMModel& geometry = getSkeletonModel()->getHFMModel();
int jointCount = rig.getJointStateCount();
_multiSphereShapes.clear();
_multiSphereShapes.reserve(jointCount);
for (int i = 0; i < jointCount; i++) {
const HFMJointShapeInfo& shapeInfo = geometry.joints[i].shapeInfo;
std::vector<btVector3> btPoints;
int lineCount = (int)shapeInfo.debugLines.size();
btPoints.reserve(lineCount);
for (int j = 0; j < lineCount; j++) {
const glm::vec3 &point = shapeInfo.debugLines[j];
auto rigPoint = scale * point;
btVector3 btPoint = glmToBullet(rigPoint);
btPoints.push_back(btPoint);
}
auto jointName = rig.nameOfJoint(i).toUpper();
MultiSphereShape multiSphereShape;
if (multiSphereShape.computeMultiSphereShape(i, jointName, btPoints)) {
multiSphereShape.calculateDebugLines();
multiSphereShape.setScale(_targetScale);
}
_multiSphereShapes.push_back(multiSphereShape);
}
}
void Avatar::updateFitBoundingBox() {
_fitBoundingBox = AABox();
if (getJointCount() == (int)_multiSphereShapes.size()) {
for (int i = 0; i < getJointCount(); i++) {
auto &shape = _multiSphereShapes[i];
glm::vec3 jointPosition;
glm::quat jointRotation;
_skeletonModel->getJointPositionInWorldFrame(i, jointPosition);
_skeletonModel->getJointRotationInWorldFrame(i, jointRotation);
_fitBoundingBox += shape.updateBoundingBox(jointPosition, jointRotation);
}
}
}
// create new model, can return an instance of a SoftAttachmentModel rather then Model
static std::shared_ptr<Model> allocateAttachmentModel(bool isSoft, const Rig& rigOverride, bool isCauterized) {
if (isSoft) {
@ -1611,6 +1730,21 @@ void Avatar::computeShapeInfo(ShapeInfo& shapeInfo) {
shapeInfo.setOffset(offset);
}
void Avatar::computeDetailedShapeInfo(ShapeInfo& shapeInfo, int jointIndex) {
if (jointIndex > -1 && jointIndex < (int)_multiSphereShapes.size()) {
auto& data = _multiSphereShapes[jointIndex].getSpheresData();
std::vector<glm::vec3> positions;
std::vector<btScalar> radiuses;
positions.reserve(data.size());
radiuses.reserve(data.size());
for (auto& sphere : data) {
positions.push_back(sphere._position);
radiuses.push_back(sphere._radius);
}
shapeInfo.setMultiSphere(positions, radiuses);
}
}
void Avatar::getCapsule(glm::vec3& start, glm::vec3& end, float& radius) {
ShapeInfo shapeInfo;
computeShapeInfo(shapeInfo);

61
libraries/avatars-renderer/src/avatars-renderer/Avatar.h
View file

@ -34,6 +34,7 @@
#include "Rig.h"
#include "MetaModelPayload.h"
#include "MultiSphereShape.h"
namespace render {
template <> const ItemKey payloadGetKey(const AvatarSharedPointer& avatar);
@ -227,12 +228,63 @@ public:
*/
Q_INVOKABLE virtual glm::vec3 getAbsoluteDefaultJointTranslationInObjectFrame(int index) const;
virtual glm::vec3 getAbsoluteJointScaleInObjectFrame(int index) const override;
virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override;
virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override;
virtual bool setAbsoluteJointRotationInObjectFrame(int index, const glm::quat& rotation) override { return false; }
virtual bool setAbsoluteJointTranslationInObjectFrame(int index, const glm::vec3& translation) override { return false; }
// world-space to avatar-space rigconversion functions
/**jsdoc
* @function MyAvatar.worldToJointPoint
* @param {Vec3} position
* @param {number} [jointIndex=-1]
* @returns {Vec3}
*/
Q_INVOKABLE glm::vec3 worldToJointPoint(const glm::vec3& position, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.worldToJointDirection
* @param {Vec3} direction
* @param {number} [jointIndex=-1]
* @returns {Vec3}
*/
Q_INVOKABLE glm::vec3 worldToJointDirection(const glm::vec3& direction, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.worldToJointRotation
* @param {Quat} rotation
* @param {number} [jointIndex=-1]
* @returns {Quat}
*/
Q_INVOKABLE glm::quat worldToJointRotation(const glm::quat& rotation, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.jointToWorldPoint
* @param {vec3} position
* @param {number} [jointIndex=-1]
* @returns {Vec3}
*/
Q_INVOKABLE glm::vec3 jointToWorldPoint(const glm::vec3& position, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.jointToWorldDirection
* @param {Vec3} direction
* @param {number} [jointIndex=-1]
* @returns {Vec3}
*/
Q_INVOKABLE glm::vec3 jointToWorldDirection(const glm::vec3& direction, const int jointIndex = -1) const;
/**jsdoc
* @function MyAvatar.jointToWorldRotation
* @param {Quat} rotation
* @param {number} [jointIndex=-1]
* @returns {Quat}
*/
Q_INVOKABLE glm::quat jointToWorldRotation(const glm::quat& rotation, const int jointIndex = -1) const;
virtual void setSkeletonModelURL(const QUrl& skeletonModelURL) override;
virtual void setAttachmentData(const QVector<AttachmentData>& attachmentData) override;
@ -320,6 +372,8 @@ public:
virtual void rebuildCollisionShape() = 0;
virtual void computeShapeInfo(ShapeInfo& shapeInfo);
virtual void computeDetailedShapeInfo(ShapeInfo& shapeInfo, int jointIndex);
void getCapsule(glm::vec3& start, glm::vec3& end, float& radius);
float computeMass();
/**jsdoc
@ -398,6 +452,7 @@ public:
float getBoundingRadius() const;
AABox getRenderBounds() const; // THis call is accessible from rendering thread only to report the bounding box of the avatar during the frame.
AABox getFitBounds() const { return _fitBoundingBox; }
void addToScene(AvatarSharedPointer self, const render::ScenePointer& scene);
void ensureInScene(AvatarSharedPointer self, const render::ScenePointer& scene);
@ -441,6 +496,7 @@ public:
void accumulateGrabPositions(std::map<QUuid, GrabLocationAccumulator>& grabAccumulators);
const std::vector<MultiSphereShape>& getMultiSphereShapes() const { return _multiSphereShapes; }
void tearDownGrabs();
signals:
@ -510,6 +566,8 @@ protected:
virtual const QString& getSessionDisplayNameForTransport() const override { return _empty; } // Save a tiny bit of bandwidth. Mixer won't look at what we send.
QString _empty{};
virtual void maybeUpdateSessionDisplayNameFromTransport(const QString& sessionDisplayName) override { _sessionDisplayName = sessionDisplayName; } // don't use no-op setter!
void computeMultiSphereShapes();
void updateFitBoundingBox();
SkeletonModelPointer _skeletonModel;
@ -632,6 +690,9 @@ protected:
static void metaBlendshapeOperator(render::ItemID renderItemID, int blendshapeNumber, const QVector<BlendshapeOffset>& blendshapeOffsets,
const QVector<int>& blendedMeshSizes, const render::ItemIDs& subItemIDs);
std::vector<MultiSphereShape> _multiSphereShapes;
AABox _fitBoundingBox;
void clearAvatarGrabData(const QUuid& grabID) override;
using SetOfIDs = std::set<QUuid>;

2
libraries/avatars/src/AvatarData.cpp
View file

@ -2900,6 +2900,7 @@ QScriptValue RayToAvatarIntersectionResultToScriptValue(QScriptEngine* engine, c
obj.setProperty("intersection", intersection);
QScriptValue surfaceNormal = vec3ToScriptValue(engine, value.surfaceNormal);
obj.setProperty("surfaceNormal", surfaceNormal);
obj.setProperty("jointIndex", value.jointIndex);
obj.setProperty("extraInfo", engine->toScriptValue(value.extraInfo));
return obj;
}
@ -2919,6 +2920,7 @@ void RayToAvatarIntersectionResultFromScriptValue(const QScriptValue& object, Ra
if (surfaceNormal.isValid()) {
vec3FromScriptValue(surfaceNormal, value.surfaceNormal);
}
value.jointIndex = object.property("jointIndex").toInt32();
value.extraInfo = object.property("extraInfo").toVariant().toMap();
}

1
libraries/avatars/src/AvatarData.h
View file

@ -1615,6 +1615,7 @@ public:
BoxFace face;
glm::vec3 intersection;
glm::vec3 surfaceNormal;
int jointIndex { -1 };
QVariantMap extraInfo;
};
Q_DECLARE_METATYPE(RayToAvatarIntersectionResult)

7
libraries/physics/src/CharacterController.cpp
View file

@ -138,7 +138,8 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
if (_dynamicsWorld) {
if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
// shouldn't fall in here, but if we do make sure both ADD and REMOVE bits are still set
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION;
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_REMOVE_FROM_SIMULATION |
PENDING_FLAG_ADD_DETAILED_TO_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
} else {
_pendingFlags &= ~PENDING_FLAG_ADD_TO_SIMULATION;
}
@ -446,10 +447,10 @@ void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const
if (_dynamicsWorld) {
// must REMOVE from world prior to shape update
_pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;
_pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION | PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION;
}
_pendingFlags |= PENDING_FLAG_UPDATE_SHAPE;
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION | PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;
}
// it's ok to change offset immediately -- there are no thread safety issues here

3
libraries/physics/src/CharacterController.h
View file

@ -32,6 +32,9 @@ const uint32_t PENDING_FLAG_UPDATE_SHAPE = 1U << 2;
const uint32_t PENDING_FLAG_JUMP = 1U << 3;
const uint32_t PENDING_FLAG_UPDATE_COLLISION_MASK = 1U << 4;
const uint32_t PENDING_FLAG_RECOMPUTE_FLYING = 1U << 5;
const uint32_t PENDING_FLAG_ADD_DETAILED_TO_SIMULATION = 1U << 6;
const uint32_t PENDING_FLAG_REMOVE_DETAILED_FROM_SIMULATION = 1U << 7;
const float DEFAULT_MIN_FLOOR_NORMAL_DOT_UP = cosf(PI / 3.0f);
class btRigidBody;

538
libraries/physics/src/MultiSphereShape.cpp Normal file
View file

@ -0,0 +1,538 @@
//
// MultiSphereShape.cpp
// libraries/physics/src
//
// Created by Luis Cuenca 5/11/2018
// Copyright 2018 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 "MultiSphereShape.h"
void SphereRegion::translate(const glm::vec3& translation) {
for (auto &line : _lines) {
line.first += translation;
line.second += translation;
}
}
void SphereRegion::dump(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines) {
for (auto &line : _lines) {
outLines.push_back(line);
}
}
void SphereRegion::insertUnique(const glm::vec3& point, std::vector<glm::vec3>& pointSet) {
auto hit = std::find_if(pointSet.begin(), pointSet.end(), [point](const glm::vec3& pointFromSet) -> bool {
return (glm::length(pointFromSet-point) < FLT_EPSILON);
});
if (hit == pointSet.end()) {
pointSet.push_back(point);
}
}
void SphereRegion::extractEdges(bool reverseY) {
if (_lines.size() == 0) {
return;
}
float yVal = _lines[0].first.y;
for (size_t i = 0; i < _lines.size(); i++) {
yVal = reverseY ? glm::max(yVal, _lines[i].first.y) : glm::min(yVal, _lines[i].first.y);
}
for (size_t i = 0; i < _lines.size(); i++) {
auto line = _lines[i];
auto p1 = line.first;
auto p2 = line.second;
auto vec = p1 - p2;
if (vec.z == 0.0f) {
insertUnique(p1, _edgesX);
insertUnique(p2, _edgesX);
} else if (vec.y == 0.0f && p1.y == yVal && p2.y == yVal) {
insertUnique(p1, _edgesY);
insertUnique(p2, _edgesY);
} else if (vec.x == 0.0f) {
insertUnique(p1, _edgesZ);
insertUnique(p2, _edgesZ);
}
}
}
void SphereRegion::extractSphereRegion(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines) {
for (size_t i = 0; i < outLines.size(); i++) {
auto &line = outLines[i];
auto &p1 = line.first;
auto &p2 = line.second;
p1.x = glm::abs(p1.x) < 0.001f ? 0.0f : p1.x;
p1.y = glm::abs(p1.y) < 0.001f ? 0.0f : p1.y;
p1.z = glm::abs(p1.z) < 0.001f ? 0.0f : p1.z;
p2.x = glm::abs(p2.x) < 0.001f ? 0.0f : p2.x;
p2.y = glm::abs(p2.y) < 0.001f ? 0.0f : p2.y;
p2.z = glm::abs(p2.z) < 0.001f ? 0.0f : p2.z;
glm::vec3 point1 = { p1.x != 0.0f ? glm::abs(p1.x) / p1.x : _direction.x,
p1.y != 0.0f ? glm::abs(p1.y) / p1.y : _direction.y,
p1.z != 0.0f ? glm::abs(p1.z) / p1.z : _direction.z };
glm::vec3 point2 = { p2.x != 0.0f ? glm::abs(p2.x) / p2.x : _direction.x,
p2.y != 0.0f ? glm::abs(p2.y) / p2.y : _direction.y,
p2.z != 0.0f ? glm::abs(p2.z) / p2.z : _direction.z };
if (point1 == _direction && point2 == _direction) {
_lines.push_back(line);
}
}
}
CollisionShapeExtractionMode MultiSphereShape::getExtractionModeByName(const QString& name) {
CollisionShapeExtractionMode mode = CollisionShapeExtractionMode::Automatic;
bool isSim = name.indexOf("SIM") == 0;
bool isFlow = name.indexOf("FLOW") == 0;
bool isEye = name.indexOf("EYE") > -1;
bool isToe = name.indexOf("TOE") > -1;
bool isShoulder = name.indexOf("SHOULDER") > -1;
bool isNeck = name.indexOf("NECK") > -1;
bool isRightHand = name == "RIGHTHAND";
bool isLeftHand = name == "LEFTHAND";
bool isRightFinger = name.indexOf("RIGHTHAND") == 0 && !isRightHand;
bool isLeftFinger = name.indexOf("LEFTHAND") == 0 && !isLeftHand;
//bool isFinger =
if (isNeck || isLeftFinger || isRightFinger) {
mode = CollisionShapeExtractionMode::SpheresY;
} else if (isShoulder) {
mode = CollisionShapeExtractionMode::SphereCollapse;
} else if (isRightHand || isLeftHand) {
mode = CollisionShapeExtractionMode::SpheresXY;
} else if (isSim || isFlow || isEye || isToe) {
mode = CollisionShapeExtractionMode::None;
}
return mode;
}
void MultiSphereShape::filterUniquePoints(const std::vector<btVector3>& kdop, std::vector<glm::vec3>& uniquePoints) {
for (size_t j = 0; j < kdop.size(); j++) {
btVector3 btPoint = kdop[j];
auto hit = std::find_if(uniquePoints.begin(), uniquePoints.end(), [btPoint](const glm::vec3& point) -> bool {
return (glm::length(btPoint.getX() - point.x) < FLT_EPSILON
&& glm::length(btPoint.getY() - point.y) < FLT_EPSILON
&& glm::length(btPoint.getZ() - point.z) < FLT_EPSILON);
});
if (hit == uniquePoints.end()) {
uniquePoints.push_back(bulletToGLM(btPoint));
}
}
}
bool MultiSphereShape::computeMultiSphereShape(int jointIndex, const QString& name, const std::vector<btVector3>& kdop, float scale) {
_scale = scale;
_jointIndex = jointIndex;
_name = name;
_mode = getExtractionModeByName(_name);
if (_mode == CollisionShapeExtractionMode::None || kdop.size() < 4 || kdop.size() > 200) {
return false;
}
std::vector<glm::vec3> points;
filterUniquePoints(kdop, points);
glm::vec3 min = glm::vec3(100.0f, 100.0f, 100.0f);
glm::vec3 max = glm::vec3(-100.0f, -100.0f, -100.0f);
_midPoint = glm::vec3(0.0f, 0.0f, 0.0f);
std::vector<glm::vec3> relPoints;
for (size_t i = 0; i < points.size(); i++) {
min.x = points[i].x < min.x ? points[i].x : min.x;
min.y = points[i].y < min.y ? points[i].y : min.y;
min.z = points[i].z < min.z ? points[i].z : min.z;
max.x = points[i].x > max.x ? points[i].x : max.x;
max.y = points[i].y > max.y ? points[i].y : max.y;
max.z = points[i].z > max.z ? points[i].z : max.z;
_midPoint += points[i];
}
_midPoint /= (int)points.size();
glm::vec3 dimensions = max - min;
for (size_t i = 0; i < points.size(); i++) {
glm::vec3 relPoint = points[i] - _midPoint;
relPoints.push_back(relPoint);
}
CollisionShapeExtractionMode applyMode = _mode;
float xCorrector = dimensions.x > dimensions.y && dimensions.x > dimensions.z ? -1.0f + (dimensions.x / (0.5f * (dimensions.y + dimensions.z))) : 0.0f;
float yCorrector = dimensions.y > dimensions.x && dimensions.y > dimensions.z ? -1.0f + (dimensions.y / (0.5f * (dimensions.x + dimensions.z))) : 0.0f;
float zCorrector = dimensions.z > dimensions.x && dimensions.z > dimensions.y ? -1.0f + (dimensions.z / (0.5f * (dimensions.x + dimensions.y))) : 0.0f;
float xyDif = glm::abs(dimensions.x - dimensions.y);
float xzDif = glm::abs(dimensions.x - dimensions.z);
float yzDif = glm::abs(dimensions.y - dimensions.z);
float xyEpsilon = (0.05f + zCorrector) * glm::max(dimensions.x, dimensions.y);
float xzEpsilon = (0.05f + yCorrector) * glm::max(dimensions.x, dimensions.z);
float yzEpsilon = (0.05f + xCorrector) * glm::max(dimensions.y, dimensions.z);
if (xyDif < 0.5f * xyEpsilon && xzDif < 0.5f * xzEpsilon && yzDif < 0.5f * yzEpsilon) {
applyMode = CollisionShapeExtractionMode::Sphere;
} else if (xzDif < xzEpsilon) {
applyMode = dimensions.y > dimensions.z ? CollisionShapeExtractionMode::SpheresY : CollisionShapeExtractionMode::SpheresXZ;
} else if (xyDif < xyEpsilon) {
applyMode = dimensions.z > dimensions.y ? CollisionShapeExtractionMode::SpheresZ : CollisionShapeExtractionMode::SpheresXY;
} else if (yzDif < yzEpsilon) {
applyMode = dimensions.x > dimensions.y ? CollisionShapeExtractionMode::SpheresX : CollisionShapeExtractionMode::SpheresYZ;
} else {
applyMode = CollisionShapeExtractionMode::SpheresXYZ;
}
if (_mode != CollisionShapeExtractionMode::Automatic && applyMode != _mode) {
bool isModeSphereAxis = (_mode >= CollisionShapeExtractionMode::SpheresX && _mode <= CollisionShapeExtractionMode::SpheresZ);
bool isApplyModeComplex = (applyMode >= CollisionShapeExtractionMode::SpheresXY && applyMode <= CollisionShapeExtractionMode::SpheresXYZ);
applyMode = (isModeSphereAxis && isApplyModeComplex) ? CollisionShapeExtractionMode::Sphere : _mode;
}
std::vector<glm::vec3> axes;
glm::vec3 axis, axis1, axis2;
SphereShapeData sphere;
switch (applyMode) {
case CollisionShapeExtractionMode::None:
break;
case CollisionShapeExtractionMode::Automatic:
break;
case CollisionShapeExtractionMode::Box:
break;
case CollisionShapeExtractionMode::Sphere:
sphere._radius = 0.5f * (dimensions.x + dimensions.y + dimensions.z) / 3.0f;
sphere._position = glm::vec3(0.0f);
_spheres.push_back(sphere);
break;
case CollisionShapeExtractionMode::SphereCollapse:
sphere._radius = 0.5f * glm::min(glm::min(dimensions.x, dimensions.y), dimensions.z);
sphere._position = glm::vec3(0.0f);
_spheres.push_back(sphere);
break;
case CollisionShapeExtractionMode::SpheresX:
axis = 0.5f* dimensions.x * Vectors::UNIT_NEG_X;
axes = { axis, -axis };
break;
case CollisionShapeExtractionMode::SpheresY:
axis = 0.5f* dimensions.y * Vectors::UNIT_NEG_Y;
axes = { axis, -axis };
break;
case CollisionShapeExtractionMode::SpheresZ:
axis = 0.5f* dimensions.z * Vectors::UNIT_NEG_Z;
axes = { axis, -axis };
break;
case CollisionShapeExtractionMode::SpheresXY:
axis1 = glm::vec3(0.5f * dimensions.x, 0.5f * dimensions.y, 0.0f);
axis2 = glm::vec3(0.5f * dimensions.x, -0.5f * dimensions.y, 0.0f);
axes = { axis1, axis2, -axis1, -axis2 };
break;
case CollisionShapeExtractionMode::SpheresYZ:
axis1 = glm::vec3(0.0f, 0.5f * dimensions.y, 0.5f * dimensions.z);
axis2 = glm::vec3(0.0f, 0.5f * dimensions.y, -0.5f * dimensions.z);
axes = { axis1, axis2, -axis1, -axis2 };
break;
case CollisionShapeExtractionMode::SpheresXZ:
axis1 = glm::vec3(0.5f * dimensions.x, 0.0f, 0.5f * dimensions.z);
axis2 = glm::vec3(-0.5f * dimensions.x, 0.0f, 0.5f * dimensions.z);
axes = { axis1, axis2, -axis1, -axis2 };
break;
case CollisionShapeExtractionMode::SpheresXYZ:
for (size_t i = 0; i < CORNER_SIGNS.size(); i++) {
axes.push_back(0.5f * (dimensions * CORNER_SIGNS[i]));
}
break;
default:
break;
}
if (axes.size() > 0) {
spheresFromAxes(relPoints, axes, _spheres);
}
for (size_t i = 0; i < _spheres.size(); i++) {
_spheres[i]._position += _midPoint;
}
return _mode != CollisionShapeExtractionMode::None;
}
void MultiSphereShape::spheresFromAxes(const std::vector<glm::vec3>& points, const std::vector<glm::vec3>& axes, std::vector<SphereShapeData>& spheres) {
float maxRadius = 0.0f;
float maxAverageRadius = 0.0f;
float minAverageRadius = glm::length(points[0]);
size_t sphereCount = axes.size();
spheres.clear();
for (size_t j = 0; j < sphereCount; j++) {
SphereShapeData sphere = SphereShapeData();
sphere._axis = axes[j];
spheres.push_back(sphere);
}
for (size_t j = 0; j < sphereCount; j++) {
auto axis = _spheres[j]._axis;
float averageRadius = 0.0f;
float maxDistance = glm::length(axis);
glm::vec3 axisDir = glm::normalize(axis);
for (size_t i = 0; i < points.size(); i++) {
float dot = glm::dot(points[i], axisDir);
if (dot > 0.0f) {
float distancePow = glm::distance2(Vectors::ZERO, points[i]);
float dotPow = glm::pow(dot, 2);
float radius = (dot / maxDistance) * glm::sqrt(distancePow - dotPow);
averageRadius += radius;
maxRadius = radius > maxRadius ? radius : maxRadius;
}
}
if (points.size() > 0) {
averageRadius /= (int)points.size();
}
maxAverageRadius = glm::max(averageRadius, maxAverageRadius);
minAverageRadius = glm::min(averageRadius, minAverageRadius);
spheres[j]._radius = averageRadius;
}
if (maxAverageRadius == 0.0f) {
maxAverageRadius = 1.0f;
}
float radiusRatio = maxRadius / maxAverageRadius;
// Push the sphere into the bounding box
float contractionRatio = 0.8f;
for (size_t j = 0; j < sphereCount; j++) {
auto axis = _spheres[j]._axis;
float distance = glm::length(axis);
float correntionRatio = radiusRatio * (spheres[j]._radius / maxAverageRadius);
float radius = (correntionRatio < 0.8f * radiusRatio ? 0.8f * radiusRatio : correntionRatio) * spheres[j]._radius;
if (sphereCount > 3) {
distance = contractionRatio * distance;
}
spheres[j]._radius = radius;
if (distance - radius > 0.0f) {
spheres[j]._position = ((distance - radius) / distance) * axis;
} else {
spheres[j]._position = glm::vec3(0.0f);
}
}
// Collapse spheres if too close
if (sphereCount == 2) {
int maxRadiusIndex = spheres[0]._radius > spheres[1]._radius ? 0 : 1;
if (glm::length(spheres[0]._position - spheres[1]._position) < 0.2f * spheres[maxRadiusIndex]._radius) {
SphereShapeData newSphere;
newSphere._position = 0.5f * (spheres[0]._position + spheres[1]._position);
newSphere._radius = spheres[maxRadiusIndex]._radius;
spheres.clear();
spheres.push_back(newSphere);
}
}
}
void MultiSphereShape::connectSpheres(int index1, int index2, bool onlyEdges) {
auto sphere1 = _spheres[index1]._radius > _spheres[index2]._radius ? _spheres[index1] : _spheres[index2];
auto sphere2 = _spheres[index1]._radius <= _spheres[index2]._radius ? _spheres[index1] : _spheres[index2];
float distance = glm::length(sphere1._position - sphere2._position);
auto axis = sphere1._position - sphere2._position;
float angleOffset = glm::asin((sphere1._radius - sphere2._radius) / distance);
float ratio1 = ((0.5f * PI) + angleOffset) / PI;
float ratio2 = ((0.5f * PI) - angleOffset) / PI;
std::vector<glm::vec3> edge1, edge2;
if (onlyEdges) {
std::vector<std::pair<glm::vec3, glm::vec3>> debugLines;
calculateSphereLines(debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), ratio1, &edge1);
calculateSphereLines(debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), ratio2, &edge2);
} else {
calculateSphereLines(_debugLines, sphere1._position, sphere1._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(axis), ratio1, &edge1);
calculateSphereLines(_debugLines, sphere2._position, sphere2._radius, DEFAULT_SPHERE_SUBDIVISIONS, glm::normalize(-axis), ratio2, &edge2);
}
connectEdges(_debugLines, edge1, edge2);
}
void MultiSphereShape::calculateDebugLines() {
if (_spheres.size() == 1) {
auto sphere = _spheres[0];
calculateSphereLines(_debugLines, sphere._position, sphere._radius);
} else if (_spheres.size() == 2) {
connectSpheres(0, 1);
} else if (_spheres.size() == 4) {
std::vector<glm::vec3> axes;
axes.resize(8);
for (size_t i = 0; i < CORNER_SIGNS.size(); i++) {
for (size_t j = 0; j < 4; j++) {
auto axis = _spheres[j]._position - _midPoint;
glm::vec3 sign = { axis.x != 0.0f ? glm::abs(axis.x) / axis.x : 0.0f,
axis.x != 0.0f ? glm::abs(axis.y) / axis.y : 0.0f ,
axis.z != 0.0f ? glm::abs(axis.z) / axis.z : 0.0f };
bool add = false;
if (sign.x == 0.0f) {
if (sign.y == CORNER_SIGNS[i].y && sign.z == CORNER_SIGNS[i].z) {
add = true;
}
} else if (sign.y == 0.0f) {
if (sign.x == CORNER_SIGNS[i].x && sign.z == CORNER_SIGNS[i].z) {
add = true;
}
} else if (sign.z == 0.0f) {
if (sign.x == CORNER_SIGNS[i].x && sign.y == CORNER_SIGNS[i].y) {
add = true;
}
} else if (sign == CORNER_SIGNS[i]) {
add = true;
}
if (add) {
axes[i] = axis;
break;
}
}
}
calculateChamferBox(_debugLines, _spheres[0]._radius, axes, _midPoint);
} else if (_spheres.size() == 8) {
std::vector<glm::vec3> axes;
for (size_t i = 0; i < _spheres.size(); i++) {
axes.push_back(_spheres[i]._position - _midPoint);
}
calculateChamferBox(_debugLines, _spheres[0]._radius, axes, _midPoint);
}
}
void MultiSphereShape::connectEdges(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const std::vector<glm::vec3>& edge1, const std::vector<glm::vec3>& edge2, bool reverse) {
if (edge1.size() == edge2.size()) {
for (size_t i = 0; i < edge1.size(); i++) {
size_t j = reverse ? edge1.size() - i - 1 : i;
outLines.push_back({ edge1[i], edge2[j] });
}
}
}
void MultiSphereShape::calculateChamferBox(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const float& radius, const std::vector<glm::vec3>& axes, const glm::vec3& translation) {
std::vector<std::pair<glm::vec3, glm::vec3>> sphereLines;
calculateSphereLines(sphereLines, glm::vec3(0.0f), radius);
std::vector<SphereRegion> regions = {
SphereRegion({ 1.0f, 1.0f, 1.0f }),
SphereRegion({ -1.0f, 1.0f, 1.0f }),
SphereRegion({ -1.0f, 1.0f, -1.0f }),
SphereRegion({ 1.0f, 1.0f, -1.0f }),
SphereRegion({ 1.0f, -1.0f, 1.0f }),
SphereRegion({ -1.0f, -1.0f, 1.0f }),
SphereRegion({ -1.0f, -1.0f, -1.0f }),
SphereRegion({ 1.0f, -1.0f, -1.0f })
};
assert(axes.size() == regions.size());
for (size_t i = 0; i < regions.size(); i++) {
regions[i].extractSphereRegion(sphereLines);
regions[i].translate(translation + axes[i]);
regions[i].extractEdges(axes[i].y < 0);
regions[i].dump(outLines);
}
connectEdges(outLines, regions[0].getEdgesZ(), regions[1].getEdgesZ());
connectEdges(outLines, regions[1].getEdgesX(), regions[2].getEdgesX());
connectEdges(outLines, regions[2].getEdgesZ(), regions[3].getEdgesZ());
connectEdges(outLines, regions[3].getEdgesX(), regions[0].getEdgesX());
connectEdges(outLines, regions[4].getEdgesZ(), regions[5].getEdgesZ());
connectEdges(outLines, regions[5].getEdgesX(), regions[6].getEdgesX());
connectEdges(outLines, regions[6].getEdgesZ(), regions[7].getEdgesZ());
connectEdges(outLines, regions[7].getEdgesX(), regions[4].getEdgesX());
connectEdges(outLines, regions[0].getEdgesY(), regions[4].getEdgesY());
connectEdges(outLines, regions[1].getEdgesY(), regions[5].getEdgesY());
connectEdges(outLines, regions[2].getEdgesY(), regions[6].getEdgesY());
connectEdges(outLines, regions[3].getEdgesY(), regions[7].getEdgesY());
}
void MultiSphereShape::calculateSphereLines(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const glm::vec3& center, const float& radius,
const int& subdivisions, const glm::vec3& direction, const float& percentage, std::vector<glm::vec3>* edge) {
float uTotalAngle = percentage * PI;
float vTotalAngle = 2.0f * PI;
int uSubdivisions = (int)glm::ceil(subdivisions * 0.5f * percentage);
int vSubdivisions = subdivisions;
float uDeltaAngle = uTotalAngle / uSubdivisions;
float vDeltaAngle = vTotalAngle / vSubdivisions;
float uAngle = 0.0f;
glm::vec3 uAxis, vAxis;
glm::vec3 mainAxis = glm::normalize(direction);
if (mainAxis.y == 1.0f || mainAxis.y == -1.0f) {
uAxis = glm::vec3(1.0f, 0.0f, 0.0f);
vAxis = glm::vec3(0.0f, 0.0f, 1.0f);
} else {
uAxis = glm::normalize(glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), mainAxis));
vAxis = glm::normalize(glm::cross(mainAxis, uAxis));
if ((uAxis.z == 0.0f && uAxis.x < 0.0f) || (uAxis.x == 0.0f && uAxis.z < 0.0f)) {
uAxis = -uAxis;
} else if (uAxis.x < 0.0f) {
uAxis = -uAxis;
}
if ((vAxis.z == 0.0f && vAxis.x < 0.0f) || (vAxis.x == 0.0f && vAxis.z < 0.0f)) {
vAxis = -vAxis;
} else if (vAxis.x < 0.0f) {
vAxis = -vAxis;
}
}
std::vector<std::vector<glm::vec3>> arcs;
auto origin = center;
for (int u = 0; u < uSubdivisions + 1; u++) {
std::vector<glm::vec3> arc;
glm::vec3 arcCenter = origin + mainAxis * (glm::cos(uAngle) * radius);
float vAngle = 0.0f;
for (int v = 0; v < vSubdivisions + 1; v++) {
float arcRadius = glm::abs(glm::sin(uAngle) * radius);
glm::vec3 arcPoint = arcCenter + (arcRadius * glm::cos(vAngle)) * uAxis + (arcRadius * glm::sin(vAngle)) * vAxis;
arc.push_back(arcPoint);
if (u == uSubdivisions && edge != nullptr) {
edge->push_back(arcPoint);
}
vAngle += vDeltaAngle;
}
arc.push_back(arc[0]);
arcs.push_back(arc);
uAngle += uDeltaAngle;
}
for (size_t i = 1; i < arcs.size(); i++) {
auto arc1 = arcs[i];
auto arc2 = arcs[i - 1];
for (size_t j = 1; j < arc1.size(); j++) {
auto point1 = arc1[j];
auto point2 = arc1[j - 1];
auto point3 = arc2[j];
std::pair<glm::vec3, glm::vec3> line1 = { point1, point2 };
std::pair<glm::vec3, glm::vec3> line2 = { point1, point3 };
outLines.push_back(line1);
outLines.push_back(line2);
}
}
}
void MultiSphereShape::setScale(float scale) {
if (scale != _scale) {
float deltaScale = scale / _scale;
for (auto& sphere : _spheres) {
sphere._axis *= deltaScale;
sphere._position *= deltaScale;
sphere._radius *= deltaScale;
}
for (auto& line : _debugLines) {
line.first *= deltaScale;
line.second *= deltaScale;
}
_scale = scale;
}
}
AABox& MultiSphereShape::updateBoundingBox(const glm::vec3& position, const glm::quat& rotation) {
_boundingBox = AABox();
auto spheres = getSpheresData();
for (size_t i = 0; i < spheres.size(); i++) {
auto sphere = spheres[i];
auto worldPosition = position + rotation * sphere._position;
glm::vec3 corner = worldPosition - glm::vec3(sphere._radius);
glm::vec3 dimensions = glm::vec3(2.0f * sphere._radius);
_boundingBox += AABox(corner, dimensions);
}
return _boundingBox;
}

112
libraries/physics/src/MultiSphereShape.h Normal file
View file

@ -0,0 +1,112 @@
//
// MultiSphereShape.h
// libraries/physics/src
//
// Created by Luis Cuenca 5/11/2018
// Copyright 2018 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
//
#ifndef hifi_MultiSphereShape_h
#define hifi_MultiSphereShape_h
#include <stdint.h>
#include <btBulletDynamicsCommon.h>
#include <GLMHelpers.h>
#include <AABox.h>
#include "BulletUtil.h"
enum CollisionShapeExtractionMode {
None = 0,
Automatic,
Box,
Sphere,
SphereCollapse,
SpheresX,
SpheresY,
SpheresZ,
SpheresXY,
SpheresYZ,
SpheresXZ,
SpheresXYZ
};
struct SphereShapeData {
SphereShapeData() {}
glm::vec3 _position;
glm::vec3 _axis;
float _radius;
};
class SphereRegion {
public:
SphereRegion() {}
SphereRegion(const glm::vec3& direction) : _direction(direction) {}
void extractSphereRegion(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines);
void extractEdges(bool reverseY = false);
void translate(const glm::vec3& translation);
void dump(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines);
const glm::vec3& getDirection() const { return _direction; }
const std::vector<glm::vec3>& getEdgesX() const { return _edgesX; }
const std::vector<glm::vec3>& getEdgesY() const { return _edgesY; }
const std::vector<glm::vec3>& getEdgesZ() const { return _edgesZ; }
private:
void insertUnique(const glm::vec3& point, std::vector<glm::vec3>& pointSet);
std::vector<std::pair<glm::vec3, glm::vec3>> _lines;
std::vector<glm::vec3> _edgesX;
std::vector<glm::vec3> _edgesY;
std::vector<glm::vec3> _edgesZ;
glm::vec3 _direction;
};
const int DEFAULT_SPHERE_SUBDIVISIONS = 16;
const std::vector<glm::vec3> CORNER_SIGNS = {
glm::vec3(1.0f, 1.0f, 1.0f), glm::vec3(-1.0f, 1.0f, 1.0f),
glm::vec3(-1.0f, 1.0f, -1.0f), glm::vec3(1.0f, 1.0f, -1.0f),
glm::vec3(1.0f, -1.0f, 1.0f), glm::vec3(-1.0f, -1.0f, 1.0f),
glm::vec3(-1.0f, -1.0f, -1.0f), glm::vec3(1.0f, -1.0f, -1.0f) };
class MultiSphereShape {
public:
MultiSphereShape() {};
bool computeMultiSphereShape(int jointIndex, const QString& name, const std::vector<btVector3>& points, float scale = 1.0f);
void calculateDebugLines();
const std::vector<SphereShapeData>& getSpheresData() const { return _spheres; }
const std::vector<std::pair<glm::vec3, glm::vec3>>& getDebugLines() const { return _debugLines; }
void setScale(float scale);
AABox& updateBoundingBox(const glm::vec3& position, const glm::quat& rotation);
const AABox& getBoundingBox() const { return _boundingBox; }
int getJointIndex() const { return _jointIndex; }
QString getJointName() const { return _name; }
bool isValid() const { return _spheres.size() > 0; }
private:
CollisionShapeExtractionMode getExtractionModeByName(const QString& name);
void filterUniquePoints(const std::vector<btVector3>& kdop, std::vector<glm::vec3>& uniquePoints);
void spheresFromAxes(const std::vector<glm::vec3>& points, const std::vector<glm::vec3>& axes,
std::vector<SphereShapeData>& spheres);
void calculateSphereLines(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const glm::vec3& center, const float& radius,
const int& subdivisions = DEFAULT_SPHERE_SUBDIVISIONS, const glm::vec3& direction = Vectors::UNIT_Y,
const float& percentage = 1.0f, std::vector<glm::vec3>* edge = nullptr);
void calculateChamferBox(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const float& radius, const std::vector<glm::vec3>& axes, const glm::vec3& translation);
void connectEdges(std::vector<std::pair<glm::vec3, glm::vec3>>& outLines, const std::vector<glm::vec3>& edge1,
const std::vector<glm::vec3>& edge2, bool reverse = false);
void connectSpheres(int index1, int index2, bool onlyEdges = false);
int _jointIndex { -1 };
QString _name;
std::vector<SphereShapeData> _spheres;
std::vector<std::pair<glm::vec3, glm::vec3>> _debugLines;
CollisionShapeExtractionMode _mode;
glm::vec3 _midPoint;
float _scale { 1.0f };
AABox _boundingBox;
};
#endif // hifi_MultiSphereShape_h

4
libraries/physics/src/ObjectMotionState.cpp
View file

@ -198,7 +198,7 @@ void ObjectMotionState::setShape(const btCollisionShape* shape) {
getShapeManager()->releaseShape(_shape);
}
_shape = shape;
if (_body) {
if (_body && _type != MOTIONSTATE_TYPE_DETAILED) {
updateCCDConfiguration();
}
}
@ -310,7 +310,7 @@ bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine*
// the shape didn't actually change, so we clear the DIRTY_SHAPE flag
flags &= ~Simulation::DIRTY_SHAPE;
// and clear the reference we just created
getShapeManager()->releaseShape(_shape);
getShapeManager()->releaseShape(_shape);
} else {
_body->setCollisionShape(const_cast<btCollisionShape*>(newShape));
setShape(newShape);

3
libraries/physics/src/ObjectMotionState.h
View file

@ -58,7 +58,8 @@ inline QString motionTypeToString(PhysicsMotionType motionType) {
enum MotionStateType {
MOTIONSTATE_TYPE_INVALID,
MOTIONSTATE_TYPE_ENTITY,
MOTIONSTATE_TYPE_AVATAR
MOTIONSTATE_TYPE_AVATAR,
MOTIONSTATE_TYPE_DETAILED
};
// The update flags trigger two varieties of updates: "hard" which require the body to be pulled

2
libraries/physics/src/PhysicsEngine.cpp
View file

@ -327,7 +327,7 @@ void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction)
}
if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) {
_activeStaticBodies.insert(object->getRigidBody());
}
}
}
// activeStaticBodies have changed (in an Easy way) and need their Aabbs updated
// but we've configured Bullet to NOT update them automatically (for improved performance)

11
libraries/physics/src/ShapeFactory.cpp
View file

@ -284,6 +284,17 @@ const btCollisionShape* ShapeFactory::createShapeFromInfo(const ShapeInfo& info)
shape = new btSphereShape(radius);
}
break;
case SHAPE_TYPE_MULTISPHERE: {
std::vector<btVector3> positions;
std::vector<float> radiuses;
auto sphereCollection = info.getSphereCollection();
for (auto &sphereData : sphereCollection) {
positions.push_back(glmToBullet(glm::vec3(sphereData)));
radiuses.push_back(sphereData.w);
}
shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size());
}
break;
case SHAPE_TYPE_ELLIPSOID: {
glm::vec3 halfExtents = info.getHalfExtents();
float radius = halfExtents.x;

43
libraries/physics/src/ThreadSafeDynamicsWorld.cpp
View file

@ -174,3 +174,46 @@ void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) {
}
}
}
void ThreadSafeDynamicsWorld::drawConnectedSpheres(btIDebugDraw* drawer, btScalar radius1, btScalar radius2, const btVector3& position1, const btVector3& position2, const btVector3& color) {
float stepRadians = PI/6.0f; // 30 degrees
btVector3 direction = position2 - position1;
btVector3 xAxis = direction.cross(btVector3(0.0f, 1.0f, 0.0f));
xAxis = xAxis.length() < EPSILON ? btVector3(1.0f, 0.0f, 0.0f) : xAxis.normalize();
btVector3 zAxis = xAxis.cross(btVector3(0.0f, 1.0f, 0.0f));
zAxis = (direction.normalize().getY() < EPSILON) ? btVector3(0.0f, 1.0f, 0.0f) : zAxis.normalize();
float fullCircle = 2.0f * PI;
for (float i = 0; i < fullCircle; i += stepRadians) {
float x1 = btSin(btScalar(i)) * radius1;
float z1 = btCos(btScalar(i)) * radius1;
float x2 = btSin(btScalar(i)) * radius2;
float z2 = btCos(btScalar(i)) * radius2;
btVector3 addVector1 = xAxis * x1 + zAxis * z1;
btVector3 addVector2 = xAxis * x2 + zAxis * z2;
drawer->drawLine(position1 + addVector1, position2 + addVector2, color);
}
}
void ThreadSafeDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) {
btCollisionWorld::debugDrawObject(worldTransform, shape, color);
if (shape->getShapeType() == MULTI_SPHERE_SHAPE_PROXYTYPE) {
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
for (int i = multiSphereShape->getSphereCount() - 1; i >= 0; i--) {
btTransform sphereTransform1, sphereTransform2;
sphereTransform1.setIdentity();
sphereTransform2.setIdentity();
int sphereIndex1 = i;
int sphereIndex2 = i > 0 ? i - 1 : multiSphereShape->getSphereCount() - 1;
sphereTransform1.setOrigin(multiSphereShape->getSpherePosition(sphereIndex1));
sphereTransform2.setOrigin(multiSphereShape->getSpherePosition(sphereIndex2));
sphereTransform1 = worldTransform * sphereTransform1;
sphereTransform2 = worldTransform * sphereTransform2;
getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(sphereIndex1), sphereTransform1, color);
drawConnectedSpheres(getDebugDrawer(), multiSphereShape->getSphereRadius(sphereIndex1), multiSphereShape->getSphereRadius(sphereIndex2), sphereTransform1.getOrigin(), sphereTransform2.getOrigin(), color);
}
} else {
btCollisionWorld::debugDrawObject(worldTransform, shape, color);
}
}

3
libraries/physics/src/ThreadSafeDynamicsWorld.h
View file

@ -53,10 +53,13 @@ public:
const VectorOfMotionStates& getDeactivatedMotionStates() const { return _deactivatedStates; }
void addChangedMotionState(ObjectMotionState* motionState) { _changedMotionStates.push_back(motionState); }
virtual void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) override;
private:
// call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState()
void synchronizeMotionState(btRigidBody* body);
void drawConnectedSpheres(btIDebugDraw* drawer, btScalar radius1, btScalar radius2, const btVector3& position1,
const btVector3& position2, const btVector3& color);
VectorOfMotionStates _changedMotionStates;
VectorOfMotionStates _deactivatedStates;

10
libraries/shared/src/DebugDraw.cpp
View file

@ -74,3 +74,13 @@ void DebugDraw::clearRays() {
Lock lock(_mapMutex);
_rays.clear();
}
void DebugDraw::drawRays(const std::vector<std::pair<glm::vec3, glm::vec3>>& lines,
const glm::vec4& color, const glm::vec3& translation, const glm::quat& rotation) {
Lock lock(_mapMutex);
for (std::pair<glm::vec3, glm::vec3> line : lines) {
auto point1 = translation + rotation * line.first;
auto point2 = translation + rotation * line.second;
_rays.push_back(Ray(point1, point2, color));
}
}

10
libraries/shared/src/DebugDraw.h
View file

@ -47,6 +47,16 @@ public:
* @param {Vec4} color - color of line, each component should be in the zero to one range. x = red, y = blue, z = green, w = alpha.
*/
Q_INVOKABLE void drawRay(const glm::vec3& start, const glm::vec3& end, const glm::vec4& color);
/**jsdoc
* Draws a line in world space, but it will only be visible for a single frame.
* @function DebugDraw.drawRay
* @param {Vec3} start - start position of line in world space.
* @param {Vec3} end - end position of line in world space.
* @param {Vec4} color - color of line, each component should be in the zero to one range. x = red, y = blue, z = green, w = alpha.
*/
Q_INVOKABLE void drawRays(const std::vector<std::pair<glm::vec3, glm::vec3>>& lines, const glm::vec4& color,
const glm::vec3& translation = glm::vec3(0.0f, 0.0f, 0.0f), const glm::quat& rotation = glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
/**jsdoc
* Adds a debug marker to the world. This marker will be drawn every frame until it is removed with DebugDraw.removeMarker.

5
libraries/shared/src/PhysicsCollisionGroups.h
View file

@ -39,6 +39,8 @@ const int32_t BULLET_COLLISION_GROUP_DYNAMIC = 1 << 1;
const int32_t BULLET_COLLISION_GROUP_KINEMATIC = 1 << 2;
const int32_t BULLET_COLLISION_GROUP_MY_AVATAR = 1 << 3;
const int32_t BULLET_COLLISION_GROUP_OTHER_AVATAR = 1 << 4;
const int32_t BULLET_COLLISION_GROUP_DETAILED_AVATAR = 1 << 5;
const int32_t BULLET_COLLISION_GROUP_DETAILED_RAY = 1 << 6;
// ...
const int32_t BULLET_COLLISION_GROUP_COLLISIONLESS = 1 << 31;
@ -64,7 +66,8 @@ const int32_t BULLET_COLLISION_MASK_MY_AVATAR = ~(BULLET_COLLISION_GROUP_COLLISI
// to move its avatar around correctly and to communicate its motion through the avatar-mixer.
// Therefore, they only need to collide against things that can be affected by their motion: dynamic and MyAvatar
const int32_t BULLET_COLLISION_MASK_OTHER_AVATAR = BULLET_COLLISION_GROUP_DYNAMIC | BULLET_COLLISION_GROUP_MY_AVATAR;
const int32_t BULLET_COLLISION_MASK_DETAILED_AVATAR = BULLET_COLLISION_GROUP_DETAILED_RAY;
const int32_t BULLET_COLLISION_MASK_DETAILED_RAY = BULLET_COLLISION_GROUP_DETAILED_AVATAR;
// COLLISIONLESS gets an empty mask.
const int32_t BULLET_COLLISION_MASK_COLLISIONLESS = 0;

4
libraries/shared/src/PhysicsHelpers.cpp
View file

@ -78,6 +78,10 @@ int32_t Physics::getDefaultCollisionMask(int32_t group) {
return BULLET_COLLISION_MASK_MY_AVATAR;
case BULLET_COLLISION_GROUP_OTHER_AVATAR:
return BULLET_COLLISION_MASK_OTHER_AVATAR;
case BULLET_COLLISION_GROUP_DETAILED_AVATAR:
return BULLET_COLLISION_MASK_DETAILED_AVATAR;
case BULLET_COLLISION_GROUP_DETAILED_RAY:
return BULLET_COLLISION_MASK_DETAILED_RAY;
default:
break;
};

31
libraries/shared/src/ShapeInfo.cpp
View file

@ -38,6 +38,7 @@
* sub-meshes.</td></tr>
* <tr><td><code>"static-mesh"</code></td><td>The exact shape of the model.</td></tr>
* <tr><td><code>"plane"</code></td><td>A plane.</td></tr>
* <tr><td><code>"multisphere"</code></td><td>A convex hull generated from a set of spheres.</td></tr>
* </tbody>
* </table>
* @typedef {string} ShapeType
@ -59,7 +60,9 @@ const char* shapeTypeNames[] = {
"simple-hull",
"simple-compound",
"static-mesh",
"ellipsoid"
"ellipsoid",
"circle",
"multisphere"
};
static const size_t SHAPETYPE_NAME_COUNT = (sizeof(shapeTypeNames) / sizeof((shapeTypeNames)[0]));
@ -90,6 +93,7 @@ void ShapeInfo::clear() {
_url.clear();
_pointCollection.clear();
_triangleIndices.clear();
_sphereCollection.clear();
_halfExtents = glm::vec3(0.0f);
_offset = glm::vec3(0.0f);
_hashKey.clear();
@ -106,6 +110,7 @@ void ShapeInfo::setParams(ShapeType type, const glm::vec3& halfExtents, QString
break;
case SHAPE_TYPE_BOX:
case SHAPE_TYPE_HULL:
case SHAPE_TYPE_MULTISPHERE:
break;
case SHAPE_TYPE_SPHERE: {
float radius = glm::length(halfExtents) / SQUARE_ROOT_OF_3;
@ -144,6 +149,17 @@ void ShapeInfo::setSphere(float radius) {
_hashKey.clear();
}
void ShapeInfo::setMultiSphere(const std::vector<glm::vec3>& centers, const std::vector<float>& radiuses) {
_url = "";
_type = SHAPE_TYPE_MULTISPHERE;
assert(centers.size() == radiuses.size() && centers.size() > 0);
for (size_t i = 0; i < centers.size(); i++) {
SphereData sphere = SphereData(centers[i], radiuses[i]);
_sphereCollection.push_back(sphere);
}
_hashKey.clear();
}
void ShapeInfo::setPointCollection(const ShapeInfo::PointCollection& pointCollection) {
_pointCollection = pointCollection;
_hashKey.clear();
@ -170,6 +186,7 @@ uint32_t ShapeInfo::getNumSubShapes() const {
case SHAPE_TYPE_COMPOUND:
case SHAPE_TYPE_SIMPLE_COMPOUND:
return _pointCollection.size();
case SHAPE_TYPE_MULTISPHERE:
case SHAPE_TYPE_SIMPLE_HULL:
case SHAPE_TYPE_STATIC_MESH:
assert(_pointCollection.size() == 1);
@ -257,7 +274,12 @@ const HashKey& ShapeInfo::getHash() const {
// The key is not yet cached therefore we must compute it.
_hashKey.hashUint64((uint64_t)_type);
if (_type != SHAPE_TYPE_SIMPLE_HULL) {
if (_type == SHAPE_TYPE_MULTISPHERE) {
for (auto &sphereData : _sphereCollection) {
_hashKey.hashVec3(glm::vec3(sphereData));
_hashKey.hashFloat(sphereData.w);
}
} else if (_type != SHAPE_TYPE_SIMPLE_HULL) {
_hashKey.hashVec3(_halfExtents);
_hashKey.hashVec3(_offset);
} else {
@ -283,9 +305,12 @@ const HashKey& ShapeInfo::getHash() const {
if (_type == SHAPE_TYPE_COMPOUND || _type == SHAPE_TYPE_SIMPLE_COMPOUND) {
uint64_t numHulls = (uint64_t)_pointCollection.size();
_hashKey.hashUint64(numHulls);
} else if (_type == SHAPE_TYPE_MULTISPHERE) {
uint64_t numSpheres = (uint64_t)_sphereCollection.size();
_hashKey.hashUint64(numSpheres);
} else if (_type == SHAPE_TYPE_SIMPLE_HULL) {
_hashKey.hashUint64(1);
}
}
}
return _hashKey;
}

10
libraries/shared/src/ShapeInfo.h
View file

@ -47,7 +47,8 @@ enum ShapeType {
SHAPE_TYPE_SIMPLE_COMPOUND,
SHAPE_TYPE_STATIC_MESH,
SHAPE_TYPE_ELLIPSOID,
SHAPE_TYPE_CIRCLE
SHAPE_TYPE_CIRCLE,
SHAPE_TYPE_MULTISPHERE
};
class ShapeInfo {
@ -57,6 +58,8 @@ public:
using PointList = QVector<glm::vec3>;
using PointCollection = QVector<PointList>;
using TriangleIndices = QVector<int32_t>;
using SphereData = glm::vec4;
using SphereCollection = QVector<SphereData>;
static QString getNameForShapeType(ShapeType type);
static ShapeType getShapeTypeForName(QString string);
@ -68,7 +71,8 @@ public:
void setSphere(float radius);
void setPointCollection(const PointCollection& pointCollection);
void setCapsuleY(float radius, float cylinderHalfHeight);
void setOffset(const glm::vec3& offset);
void setMultiSphere(const std::vector<glm::vec3>& centers, const std::vector<float>& radiuses);
void setOffset(const glm::vec3& offset);
ShapeType getType() const { return _type; }
@ -78,6 +82,7 @@ public:
PointCollection& getPointCollection() { return _pointCollection; }
const PointCollection& getPointCollection() const { return _pointCollection; }
const SphereCollection& getSphereCollection() const { return _sphereCollection; }
TriangleIndices& getTriangleIndices() { return _triangleIndices; }
const TriangleIndices& getTriangleIndices() const { return _triangleIndices; }
@ -92,6 +97,7 @@ protected:
void setHalfExtents(const glm::vec3& halfExtents);
QUrl _url; // url for model of convex collision hulls
SphereCollection _sphereCollection;
PointCollection _pointCollection;
TriangleIndices _triangleIndices;
glm::vec3 _halfExtents = glm::vec3(0.0f);