Mirrors/overte-lubosz
3
0
Fork 0
mirror of https://github.com/lubosz/overte.git synced 2025-04-24 00:13:53 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

route HMD-following through the physics simulation

Browse source
This commit is contained in:
Andrew Meadows 2015-10-30 18:59:33 -07:00 committed by U-GAPOS\andrew
parent 9b1b009b8b
commit fd557c4e52
4 changed files with 125 additions and 116 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -157,6 +157,8 @@ void MyAvatar::reset(bool andReload) {
// Reset dynamic state.
_wasPushing = _isPushing = _isBraking = _billboardValid = false;
_isFollowingHMD = false;
_hmdFollowVelocity = Vectors::ZERO;
_hmdFollowSpeed = 0.0f;
_skeletonModel.reset();
getHead()->reset();
_targetVelocity = glm::vec3(0.0f);
@ -332,25 +334,6 @@ glm::mat4 MyAvatar::getSensorToWorldMatrix() const {
return _sensorToWorldMatrix;
}
// returns true if pos is OUTSIDE of the vertical capsule
// where the middle cylinder length is defined by capsuleLen and the radius by capsuleRad.
static bool pointIsOutsideCapsule(const glm::vec3& pos, float capsuleLen, float capsuleRad) {
const float halfCapsuleLen = capsuleLen / 2.0f;
if (fabs(pos.y) <= halfCapsuleLen) {
// cylinder check for middle capsule
glm::vec2 horizPos(pos.x, pos.z);
return glm::length(horizPos) > capsuleRad;
} else if (pos.y > halfCapsuleLen) {
glm::vec3 center(0.0f, halfCapsuleLen, 0.0f);
return glm::length(center - pos) > capsuleRad;
} else if (pos.y < halfCapsuleLen) {
glm::vec3 center(0.0f, -halfCapsuleLen, 0.0f);
return glm::length(center - pos) > capsuleRad;
} else {
return false;
}
}
// Pass a recent sample of the HMD to the avatar.
// This can also update the avatar's position to follow the HMD
// as it moves through the world.
@ -359,102 +342,58 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
_hmdSensorMatrix = hmdSensorMatrix;
_hmdSensorPosition = extractTranslation(hmdSensorMatrix);
_hmdSensorOrientation = glm::quat_cast(hmdSensorMatrix);
}
// calc deltaTime
auto now = usecTimestampNow();
auto deltaUsecs = now - _lastUpdateFromHMDTime;
_lastUpdateFromHMDTime = now;
double actualDeltaTime = (double)deltaUsecs / (double)USECS_PER_SECOND;
const float BIGGEST_DELTA_TIME_SECS = 0.25f;
float deltaTime = glm::clamp((float)actualDeltaTime, 0.0f, BIGGEST_DELTA_TIME_SECS);
bool hmdIsAtRest = _hmdAtRestDetector.update(deltaTime, _hmdSensorPosition, _hmdSensorOrientation);
// It can be more accurate/smooth to use velocity rather than position,
// but some modes (e.g., hmd standing) update position without updating velocity.
// So, let's create our own workingVelocity from the worldPosition...
glm::vec3 positionDelta = getPosition() - _lastPosition;
glm::vec3 workingVelocity = positionDelta / deltaTime;
_lastPosition = getPosition();
const float MOVE_ENTER_SPEED_THRESHOLD = 0.2f; // m/sec
const float MOVE_EXIT_SPEED_THRESHOLD = 0.07f; // m/sec
void MyAvatar::updateHMDFollowVelocity() {
bool isMoving;
if (_lastIsMoving) {
isMoving = glm::length(workingVelocity) >= MOVE_EXIT_SPEED_THRESHOLD;
const float MOVE_EXIT_SPEED_THRESHOLD = 0.07f; // m/sec
isMoving = glm::length(_velocity) >= MOVE_EXIT_SPEED_THRESHOLD;
} else {
isMoving = glm::length(workingVelocity) > MOVE_ENTER_SPEED_THRESHOLD;
const float MOVE_ENTER_SPEED_THRESHOLD = 0.2f; // m/sec
isMoving = glm::length(_velocity) > MOVE_ENTER_SPEED_THRESHOLD;
}
bool justStartedMoving = (_lastIsMoving != isMoving) && isMoving;
_lastIsMoving = isMoving;
if (shouldFollowHMD() || hmdIsAtRest || justStartedMoving) {
beginFollowingHMD();
}
followHMD(deltaTime);
}
glm::vec3 MyAvatar::getHMDCorrectionVelocity() const {
// TODO: impelement this
return Vectors::ZERO;
}
void MyAvatar::beginFollowingHMD() {
// begin homing toward derived body position.
if (!_isFollowingHMD) {
bool hmdIsAtRest = _hmdAtRestDetector.update(_hmdSensorPosition, _hmdSensorOrientation);
if (hmdIsAtRest || justStartedMoving) {
_isFollowingHMD = true;
_followHMDAlpha = 0.0f;
}
}
bool MyAvatar::shouldFollowHMD() const {
if (!_isFollowingHMD) {
// define a vertical capsule
const float FOLLOW_HMD_CAPSULE_RADIUS = 0.2f; // meters
const float FOLLOW_HMD_CAPSULE_LENGTH = 0.05f; // length of the cylinder part of the capsule in meters.
// detect if the derived body position is outside of a capsule around the _bodySensorMatrix
auto newBodySensorMatrix = deriveBodyFromHMDSensor();
glm::vec3 localPoint = extractTranslation(newBodySensorMatrix) - extractTranslation(_bodySensorMatrix);
return pointIsOutsideCapsule(localPoint, FOLLOW_HMD_CAPSULE_LENGTH, FOLLOW_HMD_CAPSULE_RADIUS);
// compute offset to body's target position (in sensor-frame)
auto sensorBodyMatrix = deriveBodyFromHMDSensor();
_hmdFollowOffset = extractTranslation(sensorBodyMatrix) - extractTranslation(_bodySensorMatrix);
if (_hmdFollowOffset.y < 0.0f) {
// don't pull the body DOWN to match the target (allow animation system to squat)
_hmdFollowOffset.y = 0.0f;
}
return false;
}
void MyAvatar::followHMD(float deltaTime) {
if (_isFollowingHMD) {
const float FOLLOW_HMD_DURATION = 0.5f; // seconds
auto newBodySensorMatrix = deriveBodyFromHMDSensor();
auto worldBodyMatrix = _sensorToWorldMatrix * newBodySensorMatrix;
glm::vec3 worldBodyPos = extractTranslation(worldBodyMatrix);
glm::quat worldBodyRot = glm::normalize(glm::quat_cast(worldBodyMatrix));
_followHMDAlpha += (1.0f / FOLLOW_HMD_DURATION) * deltaTime;
if (_followHMDAlpha >= 1.0f) {
_isFollowingHMD = false;
nextAttitude(worldBodyPos, worldBodyRot);
_bodySensorMatrix = newBodySensorMatrix;
} else {
// interp position toward the desired pos
glm::vec3 pos = lerp(getPosition(), worldBodyPos, _followHMDAlpha);
glm::quat rot = glm::normalize(safeMix(getOrientation(), worldBodyRot, _followHMDAlpha));
nextAttitude(pos, rot);
// interp sensor matrix toward desired
glm::vec3 nextBodyPos = extractTranslation(newBodySensorMatrix);
glm::quat nextBodyRot = glm::normalize(glm::quat_cast(newBodySensorMatrix));
glm::vec3 prevBodyPos = extractTranslation(_bodySensorMatrix);
glm::quat prevBodyRot = glm::normalize(glm::quat_cast(_bodySensorMatrix));
pos = lerp(prevBodyPos, nextBodyPos, _followHMDAlpha);
rot = glm::normalize(safeMix(prevBodyRot, nextBodyRot, _followHMDAlpha));
_bodySensorMatrix = createMatFromQuatAndPos(rot, pos);
bool needNewFollowSpeed = (_isFollowingHMD && _hmdFollowSpeed == 0.0f);
if (!needNewFollowSpeed) {
// check to see if offset has exceeded its threshold
float distance = glm::length(_hmdFollowOffset);
const float MAX_HMD_HIP_SHIFT = 0.2f;
if (distance > MAX_HMD_HIP_SHIFT) {
_isFollowingHMD = true;
needNewFollowSpeed = true;
}
}
if (_isFollowingHMD) {
// only bother to rotate into world frame if we're following
glm::quat sensorToWorldRotation = extractRotation(_sensorToWorldMatrix);
_hmdFollowOffset = sensorToWorldRotation * _hmdFollowOffset;
}
if (needNewFollowSpeed) {
// compute new velocity that will be used to resolve offset of hips from body
const float FOLLOW_HMD_DURATION = 0.5f; // seconds
_hmdFollowVelocity = (_hmdFollowOffset / FOLLOW_HMD_DURATION);
_hmdFollowSpeed = glm::length(_hmdFollowVelocity);
} else if (_isFollowingHMD) {
// compute new velocity (but not new speed)
_hmdFollowVelocity = _hmdFollowSpeed * glm::normalize(_hmdFollowOffset);
}
}
// best called at end of main loop, just before rendering.
@ -1356,7 +1295,8 @@ void MyAvatar::prepareForPhysicsSimulation() {
relayDriveKeysToCharacterController();
_characterController.setTargetVelocity(getTargetVelocity());
_characterController.setAvatarPositionAndOrientation(getPosition(), getOrientation());
_characterController.setHMDVelocity(getHMDCorrectionVelocity());
updateHMDFollowVelocity();
_characterController.setHMDVelocity(_hmdFollowVelocity);
}
void MyAvatar::harvestResultsFromPhysicsSimulation() {
@ -1364,9 +1304,54 @@ void MyAvatar::harvestResultsFromPhysicsSimulation() {
glm::quat orientation = getOrientation();
_characterController.getAvatarPositionAndOrientation(position, orientation);
nextAttitude(position, orientation);
setVelocity(_characterController.getLinearVelocity());
// TODO: harvest HMD shift here
//glm::vec3 hmdShift = _characterController.getHMDShift();
if (_isFollowingHMD) {
setVelocity(_characterController.getLinearVelocity() + _hmdFollowVelocity);
glm::vec3 hmdShift = _characterController.getHMDShift();
adjustSensorTransform(hmdShift);
} else {
setVelocity(_characterController.getLinearVelocity());
}
}
void MyAvatar::adjustSensorTransform(glm::vec3 hmdShift) {
// compute blendFactor of latest hmdShift
// which we'll use to blend the rotation part
float blendFactor = 1.0f;
float shiftLength = glm::length(hmdShift);
if (shiftLength > 1.0e-5f) {
float offsetLength = glm::length(_hmdFollowOffset);
if (offsetLength > shiftLength) {
blendFactor = shiftLength / offsetLength;
}
}
auto newBodySensorMatrix = deriveBodyFromHMDSensor();
auto worldBodyMatrix = _sensorToWorldMatrix * newBodySensorMatrix;
glm::quat finalBodyRotation = glm::normalize(glm::quat_cast(worldBodyMatrix));
if (blendFactor >= 0.99f) {
// the "adjustment" is more or less complete so stop following
_isFollowingHMD = false;
_hmdFollowSpeed = 0.0f;
// and slam the body's transform anyway to eliminate any slight errors
glm::vec3 finalBodyPosition = extractTranslation(worldBodyMatrix);
nextAttitude(finalBodyPosition, finalBodyRotation);
_bodySensorMatrix = newBodySensorMatrix;
} else {
// physics already did the positional blending for us
glm::vec3 newBodyPosition = getPosition();
// but the rotational part must be done manually
glm::quat newBodyRotation = glm::normalize(safeMix(getOrientation(), finalBodyRotation, blendFactor));
nextAttitude(newBodyPosition, newBodyRotation);
// interp sensor matrix toward the desired
glm::vec3 prevPosition = extractTranslation(_bodySensorMatrix);
glm::quat prevRotation = glm::normalize(glm::quat_cast(_bodySensorMatrix));
glm::vec3 nextPosition = extractTranslation(newBodySensorMatrix);
glm::quat nextRotation = glm::normalize(glm::quat_cast(newBodySensorMatrix));
_bodySensorMatrix = createMatFromQuatAndPos(
glm::normalize(safeMix(prevRotation, nextRotation, blendFactor)),
lerp(prevPosition, nextPosition, blendFactor));
}
}
QString MyAvatar::getScriptedMotorFrame() const {

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

@ -101,8 +101,6 @@ public:
// as it moves through the world.
void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix);
glm::vec3 getHMDCorrectionVelocity() const;
// best called at end of main loop, just before rendering.
// update sensor to world matrix from current body position and hmd sensor.
// This is so the correct camera can be used for rendering.
@ -211,6 +209,7 @@ public:
void prepareForPhysicsSimulation();
void harvestResultsFromPhysicsSimulation();
void adjustSensorTransform(glm::vec3 hmdShift);
const QString& getCollisionSoundURL() { return _collisionSoundURL; }
void setCollisionSoundURL(const QString& url);
@ -316,9 +315,10 @@ private:
const RecorderPointer getRecorder() const { return _recorder; }
const PlayerPointer getPlayer() const { return _player; }
void beginFollowingHMD();
bool shouldFollowHMD() const;
void followHMD(float deltaTime);
//void beginFollowingHMD();
//bool shouldFollowHMD() const;
//void followHMD(float deltaTime);
void updateHMDFollowVelocity();
bool cameraInsideHead() const;
@ -395,6 +395,9 @@ private:
// used to transform any sensor into world space, including the _hmdSensorMat, or hand controllers.
glm::mat4 _sensorToWorldMatrix;
glm::vec3 _hmdFollowOffset { Vectors::ZERO };
glm::vec3 _hmdFollowVelocity { Vectors::ZERO };
float _hmdFollowSpeed { 0.0f };
bool _goToPending;
glm::vec3 _goToPosition;
@ -415,9 +418,7 @@ private:
bool _isFollowingHMD { false };
float _followHMDAlpha { 0.0f };
quint64 _lastUpdateFromHMDTime { usecTimestampNow() };
AtRestDetector _hmdAtRestDetector;
glm::vec3 _lastPosition;
bool _lastIsMoving { false };
quint64 _lastStepPulse { 0 };
bool _pulseUpdate { false };

24
libraries/shared/src/AtRestDetector.cpp
View file

@ -1,5 +1,19 @@
//
// AtRestDetector.cpp
// libraries/shared/src
//
// Created by Anthony Thibault on 10/6/2015
// Copyright 2015 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 "AtRestDetector.h"
#include "NumericalConstants.h"
#include "SharedLogging.h"
#include "SharedUtil.h"
AtRestDetector::AtRestDetector(const glm::vec3& startPosition, const glm::quat& startRotation) {
reset(startPosition, startRotation);
@ -12,9 +26,14 @@ void AtRestDetector::reset(const glm::vec3& startPosition, const glm::quat& star
glm::quat ql = glm::log(startRotation);
_quatLogAverage = glm::vec3(ql.x, ql.y, ql.z);
_quatLogVariance = 0.0f;
_lastUpdateTime = usecTimestampNow();
_isAtRest = false;
}
bool AtRestDetector::update(float dt, const glm::vec3& position, const glm::quat& rotation) {
bool AtRestDetector::update(const glm::vec3& position, const glm::quat& rotation) {
uint64_t now = usecTimestampNow();
float dt = (float)(_lastUpdateTime - now) / (float)USECS_PER_SECOND;
_lastUpdateTime = now;
const float TAU = 1.0f;
float delta = glm::min(dt / TAU, 1.0f);
@ -37,5 +56,6 @@ bool AtRestDetector::update(float dt, const glm::vec3& position, const glm::quat
const float POSITION_VARIANCE_THRESHOLD = 0.001f;
const float QUAT_LOG_VARIANCE_THRESHOLD = 0.00002f;
return _positionVariance < POSITION_VARIANCE_THRESHOLD && _quatLogVariance < QUAT_LOG_VARIANCE_THRESHOLD;
_isAtRest = _positionVariance < POSITION_VARIANCE_THRESHOLD && _quatLogVariance < QUAT_LOG_VARIANCE_THRESHOLD;
return _isAtRest;
}

11
libraries/shared/src/AtRestDetector.h
View file

@ -21,14 +21,17 @@ public:
void reset(const glm::vec3& startPosition, const glm::quat& startRotation);
// returns true if object is at rest, dt in assumed to be seconds.
bool update(float dt, const glm::vec3& position, const glm::quat& startRotation);
bool update(const glm::vec3& position, const glm::quat& startRotation);
bool isAtRest() const { return _isAtRest; }
protected:
glm::vec3 _positionAverage;
float _positionVariance;
glm::vec3 _quatLogAverage;
float _quatLogVariance;
uint64_t _lastUpdateTime { 0 };
float _positionVariance { 0.0f };
float _quatLogVariance { 0.0f };
bool _isAtRest { false };
};
#endif