Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-16 22:06:18 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

Merge pull request #8691 from highfidelity/out-of-body-experience

Browse source 
Room Scale HMD comfort features.
This commit is contained in:
Seth Alves 2016-12-01 11:09:39 -08:00 committed by GitHub
commit efe9571ab8
63 changed files with 2522 additions and 548 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

45
interface/resources/avatar/avatar-animation.json
View file

@ -255,10 +255,10 @@
},
{
"id": "idleToWalkFwd",
"interpTarget": 3,
"interpDuration": 3,
"interpTarget": 4,
"interpDuration": 3.0,
"transitions": [
{ "var": "idleToWalkFwdOnDone", "state": "walkFwd" },
{ "var": "idleToWalkFwdClipOnDone", "state": "walkFwd" },
{ "var": "isNotMoving", "state": "idle" },
{ "var": "isMovingBackward", "state": "walkBwd" },
{ "var": "isMovingRight", "state": "strafeRight" },
@ -292,8 +292,8 @@
},
{
"id": "walkBwd",
"interpTarget": 6,
"interpDuration": 6,
"interpTarget": 4,
"interpDuration": 4,
"transitions": [
{ "var": "isNotMoving", "state": "idle" },
{ "var": "isMovingForward", "state": "walkFwd" },
@ -523,7 +523,7 @@
"data": {
"alpha": 0.0,
"desiredSpeed": 1.4,
"characteristicSpeeds": [0.5, 1.4, 4.5],
"characteristicSpeeds": [0.5, 1.3, 4.5],
"alphaVar": "moveForwardAlpha",
"desiredSpeedVar": "moveForwardSpeed"
},
@ -568,15 +568,28 @@
},
{
"id": "idleToWalkFwd",
"type": "clip",
"type": "blendLinearMove",
"data": {
"url": "animations/idle_to_walk.fbx",
"startFrame": 1.0,
"endFrame": 13.0,
"timeScale": 1.0,
"loopFlag": false
"alpha": 0.0,
"desiredSpeed": 1.2,
"characteristicSpeeds": [1.2],
"alphaVar": 0.0,
"desiredSpeedVar": "moveForwardSpeed"
},
"children": []
"children": [
{
"id": "idleToWalkFwdClip",
"type": "clip",
"data": {
"url": "animations/idle_to_walk.fbx",
"startFrame": 1.0,
"endFrame": 13.0,
"timeScale": 0.9,
"loopFlag": false
},
"children": []
}
]
},
{
"id": "walkBwd",
@ -584,7 +597,7 @@
"data": {
"alpha": 0.0,
"desiredSpeed": 1.4,
"characteristicSpeeds": [0.6, 1.45],
"characteristicSpeeds": [0.6, 1.05],
"alphaVar": "moveBackwardAlpha",
"desiredSpeedVar": "moveBackwardSpeed"
},
@ -646,7 +659,7 @@
"data": {
"alpha": 0.0,
"desiredSpeed": 1.4,
"characteristicSpeeds": [0.2, 0.65],
"characteristicSpeeds": [0.2, 0.5],
"alphaVar": "moveLateralAlpha",
"desiredSpeedVar": "moveLateralSpeed"
},
@ -683,7 +696,7 @@
"data": {
"alpha": 0.0,
"desiredSpeed": 1.4,
"characteristicSpeeds": [0.2, 0.65],
"characteristicSpeeds": [0.2, 0.5],
"alphaVar": "moveLateralAlpha",
"desiredSpeedVar": "moveLateralSpeed"
},

9
interface/src/Application.cpp
View file

@ -3894,13 +3894,6 @@ void Application::update(float deltaTime) {
if (nearbyEntitiesAreReadyForPhysics()) {
_physicsEnabled = true;
getMyAvatar()->updateMotionBehaviorFromMenu();
} else {
auto characterController = getMyAvatar()->getCharacterController();
if (characterController) {
// if we have a character controller, disable it here so the avatar doesn't get stuck due to
// a non-loading collision hull.
characterController->setEnabled(false);
}
}
}
}
@ -4029,7 +4022,7 @@ void Application::update(float deltaTime) {
avatarManager->getObjectsToChange(motionStates);
_physicsEngine->changeObjects(motionStates);
myAvatar->prepareForPhysicsSimulation();
myAvatar->prepareForPhysicsSimulation(deltaTime);
_physicsEngine->forEachAction([&](EntityActionPointer action) {
action->prepareForPhysicsSimulation();
});

16
interface/src/Menu.cpp
View file

@ -498,6 +498,10 @@ Menu::Menu() {
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderMyLookAtVectors, 0, false);
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderOtherLookAtVectors, 0, false);
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::FixGaze, 0, false);
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::DisplayLeftFootTrace, 0, false,
avatar.get(), SLOT(setEnableDebugDrawLeftFootTrace(bool)));
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::DisplayRightFootTrace, 0, false,
avatar.get(), SLOT(setEnableDebugDrawRightFootTrace(bool)));
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawDefaultPose, 0, false,
avatar.get(), SLOT(setEnableDebugDrawDefaultPose(bool)));
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::AnimDebugDrawAnimPose, 0, false,
@ -514,6 +518,8 @@ Menu::Menu() {
avatar.get(), SLOT(setEnableInverseKinematics(bool)));
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderSensorToWorldMatrix, 0, false,
avatar.get(), SLOT(setEnableDebugDrawSensorToWorldMatrix(bool)));
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKTargets, 0, false,
avatar.get(), SLOT(setEnableDebugDrawIKTargets(bool)));
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::ActionMotorControl,
Qt::CTRL | Qt::SHIFT | Qt::Key_K, true, avatar.get(), SLOT(updateMotionBehaviorFromMenu()),
@ -523,10 +529,18 @@ Menu::Menu() {
avatar.get(), SLOT(updateMotionBehaviorFromMenu()),
UNSPECIFIED_POSITION, "Developer");
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableCharacterController, 0, true,
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableAvatarCollisions, 0, true,
avatar.get(), SLOT(updateMotionBehaviorFromMenu()),
UNSPECIFIED_POSITION, "Developer");
// KINEMATIC_CONTROLLER_HACK
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::MoveKinematically, 0, false,
avatar.get(), SLOT(updateMotionBehaviorFromMenu()),
UNSPECIFIED_POSITION, "Developer");
addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::EnableVerticalComfortMode, 0, false,
avatar.get(), SLOT(setEnableVerticalComfortMode(bool)));
// Developer > Hands >>>
MenuWrapper* handOptionsMenu = developerMenu->addMenu("Hands");
addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false,

9
interface/src/Menu.h
View file

@ -87,6 +87,8 @@ namespace MenuOption {
const QString DiskCacheEditor = "Disk Cache Editor";
const QString DisplayCrashOptions = "Display Crash Options";
const QString DisplayHandTargets = "Show Hand Targets";
const QString DisplayLeftFootTrace = "Show Left Foot Trace";
const QString DisplayRightFootTrace = "Show Right Foot Trace";
const QString DisplayModelBounds = "Display Model Bounds";
const QString DisplayModelTriangles = "Display Model Triangles";
const QString DisplayModelElementChildProxies = "Display Model Element Children";
@ -96,8 +98,11 @@ namespace MenuOption {
const QString DontRenderEntitiesAsScene = "Don't Render Entities as Scene";
const QString EchoLocalAudio = "Echo Local Audio";
const QString EchoServerAudio = "Echo Server Audio";
const QString EnableCharacterController = "Enable avatar collisions";
const QString EnableAvatarCollisions = "Enable Avatar Collisions";
const QString EnableIncrementalTextureTransfer = "Enable Incremental Texture Transfer";
const QString EnableDynamicTextureManagement = "Enable Dynamic Texture Management";
const QString EnableInverseKinematics = "Enable Inverse Kinematics";
const QString EnableVerticalComfortMode = "Enable Vertical Comfort Mode";
const QString ExpandMyAvatarSimulateTiming = "Expand /myAvatar/simulation";
const QString ExpandMyAvatarTiming = "Expand /myAvatar";
const QString ExpandOtherAvatarTiming = "Expand /otherAvatar";
@ -163,6 +168,7 @@ namespace MenuOption {
const QString RenderResolutionThird = "1/3";
const QString RenderResolutionQuarter = "1/4";
const QString RenderSensorToWorldMatrix = "Show SensorToWorld Matrix";
const QString RenderIKTargets = "Show IK Targets";
const QString ResetAvatarSize = "Reset Avatar Size";
const QString ResetSensors = "Reset Sensors";
const QString RunningScripts = "Running Scripts...";
@ -191,6 +197,7 @@ namespace MenuOption {
const QString UseAudioForMouth = "Use Audio for Mouth";
const QString UseCamera = "Use Camera";
const QString UseAnimPreAndPostRotations = "Use Anim Pre and Post Rotations";
const QString MoveKinematically = "Move Kinematically"; // KINEMATIC_CONTROLLER_HACK
const QString VelocityFilter = "Velocity Filter";
const QString VisibleToEveryone = "Everyone";
const QString VisibleToFriends = "Friends";

463
interface/src/avatar/MyAvatar.cpp Normal file → Executable file
View file

@ -36,6 +36,7 @@
#include <UserActivityLogger.h>
#include <AnimDebugDraw.h>
#include <AnimClip.h>
#include <AnimUtil.h>
#include <recording/Deck.h>
#include <recording/Recorder.h>
#include <recording/Clip.h>
@ -60,7 +61,7 @@ using namespace std;
const glm::vec3 DEFAULT_UP_DIRECTION(0.0f, 1.0f, 0.0f);
const float DEFAULT_REAL_WORLD_FIELD_OF_VIEW_DEGREES = 30.0f;
const float MAX_WALKING_SPEED = 2.6f; // human walking speed
const float MAX_WALKING_SPEED = 2.0f; // human walking speed
const float MAX_BOOST_SPEED = 0.5f * MAX_WALKING_SPEED; // action motor gets additive boost below this speed
const float MIN_AVATAR_SPEED = 0.05f;
const float MIN_AVATAR_SPEED_SQUARED = MIN_AVATAR_SPEED * MIN_AVATAR_SPEED; // speed is set to zero below this
@ -83,6 +84,13 @@ const float MyAvatar::ZOOM_MIN = 0.5f;
const float MyAvatar::ZOOM_MAX = 25.0f;
const float MyAvatar::ZOOM_DEFAULT = 1.5f;
// OUTOFBODY_HACK defined in SkeletonModel.cpp
extern glm::vec3 TRUNCATE_IK_CAPSULE_POSITION;
extern float TRUNCATE_IK_CAPSULE_LENGTH;
extern float TRUNCATE_IK_CAPSULE_RADIUS;
extern float MIN_OUT_OF_BODY_DISTANCE;
extern float MAX_OUT_OF_BODY_DISTANCE;
MyAvatar::MyAvatar(RigPointer rig) :
Avatar(rig),
_wasPushing(false),
@ -139,8 +147,6 @@ MyAvatar::MyAvatar(RigPointer rig) :
// when we leave a domain we lift whatever restrictions that domain may have placed on our scale
connect(&domainHandler, &DomainHandler::disconnectedFromDomain, this, &MyAvatar::clearScaleRestriction);
_characterController.setEnabled(true);
_bodySensorMatrix = deriveBodyFromHMDSensor();
using namespace recording;
@ -370,6 +376,27 @@ void MyAvatar::update(float deltaTime) {
simulate(deltaTime);
// Request to show the hand controllers if we're out-of-body for more then HAND_CONTROLLER_SHOW_TIME.
// Similarlly request to hide the controllers when we return to our bodies.
const float HAND_CONTROLLER_SHOW_TIME = 0.75f;
auto hmdInterface = DependencyManager::get<HMDScriptingInterface>();
if (isOutOfBody() != _handControllerShow) {
_handControllerShowTimer += deltaTime;
if (_handControllerShowTimer > HAND_CONTROLLER_SHOW_TIME) {
if (isOutOfBody()) {
hmdInterface->requestShowHandControllers();
_handControllerShow = true;
_handControllerShowTimer = 0.0f;
} else {
hmdInterface->requestHideHandControllers();
_handControllerShow = false;
_handControllerShowTimer = 0.0f;
}
}
} else {
_handControllerShowTimer = 0.0f;
}
currentEnergy += energyChargeRate;
currentEnergy -= getAccelerationEnergy();
currentEnergy -= getAudioEnergy();
@ -437,10 +464,18 @@ void MyAvatar::simulate(float deltaTime) {
// update sensorToWorldMatrix for camera and hand controllers
// before we perform rig animations and IK.
updateSensorToWorldMatrix();
if (_characterController.getState() != CharacterController::State::Hover) {
updateSensorToWorldMatrix(_enableVerticalComfortMode ? SensorToWorldUpdateMode::VerticalComfort : SensorToWorldUpdateMode::Vertical);
}
{
PerformanceTimer perfTimer("skeleton");
if (_rig) {
_rig->setEnableDebugDrawIKTargets(_enableDebugDrawIKTargets);
}
_skeletonModel->simulate(deltaTime);
}
@ -521,8 +556,8 @@ void MyAvatar::simulate(float deltaTime) {
}
});
_characterController.setFlyingAllowed(flyingAllowed);
if (!_characterController.isEnabled() && !ghostingAllowed) {
_characterController.setEnabled(true);
if (!ghostingAllowed && _characterController.getCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS) {
_characterController.setCollisionGroup(BULLET_COLLISION_GROUP_MY_AVATAR);
}
}
@ -565,11 +600,27 @@ void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeV
// best called at end of main loop, after physics.
// update sensor to world matrix from current body position and hmd sensor.
// This is so the correct camera can be used for rendering.
void MyAvatar::updateSensorToWorldMatrix() {
// update the sensor mat so that the body position will end up in the desired
// position when driven from the head.
glm::mat4 desiredMat = createMatFromQuatAndPos(getOrientation(), getPosition());
_sensorToWorldMatrix = desiredMat * glm::inverse(_bodySensorMatrix);
void MyAvatar::updateSensorToWorldMatrix(SensorToWorldUpdateMode mode) {
if (mode == SensorToWorldUpdateMode::Full) {
glm::mat4 bodyToWorld = createMatFromQuatAndPos(getOrientation(), getPosition());
setSensorToWorldMatrix(bodyToWorld * glm::inverse(_bodySensorMatrix));
} else if (mode == SensorToWorldUpdateMode::Vertical ||
mode == SensorToWorldUpdateMode::VerticalComfort) {
glm::mat4 bodyToWorld = createMatFromQuatAndPos(getOrientation(), getPosition());
glm::mat4 newSensorToWorldMat = bodyToWorld * glm::inverse(_bodySensorMatrix);
glm::mat4 sensorToWorldMat = _sensorToWorldMatrix;
sensorToWorldMat[3][1] = newSensorToWorldMat[3][1];
if (mode == SensorToWorldUpdateMode::VerticalComfort &&
fabsf(_sensorToWorldMatrix[3][1] - newSensorToWorldMat[3][1]) > 0.1f) {
setSensorToWorldMatrix(sensorToWorldMat);
} else if (mode == SensorToWorldUpdateMode::Vertical) {
setSensorToWorldMatrix(sensorToWorldMat);
}
}
}
void MyAvatar::setSensorToWorldMatrix(const glm::mat4& sensorToWorld) {
_sensorToWorldMatrix = sensorToWorld;
lateUpdatePalms();
@ -811,6 +862,14 @@ float loadSetting(Settings& settings, const QString& name, float defaultValue) {
return value;
}
void MyAvatar::setEnableDebugDrawLeftFootTrace(bool isEnabled) {
_enableDebugDrawLeftFootTrace = isEnabled;
}
void MyAvatar::setEnableDebugDrawRightFootTrace(bool isEnabled) {
_enableDebugDrawRightFootTrace = isEnabled;
}
void MyAvatar::setEnableDebugDrawDefaultPose(bool isEnabled) {
_enableDebugDrawDefaultPose = isEnabled;
@ -853,6 +912,11 @@ void MyAvatar::setEnableDebugDrawSensorToWorldMatrix(bool isEnabled) {
}
}
void MyAvatar::setEnableDebugDrawIKTargets(bool isEnabled) {
_enableDebugDrawIKTargets = isEnabled;
}
void MyAvatar::setEnableMeshVisible(bool isEnabled) {
render::ScenePointer scene = qApp->getMain3DScene();
_skeletonModel->setVisibleInScene(isEnabled, scene);
@ -867,6 +931,10 @@ void MyAvatar::setEnableInverseKinematics(bool isEnabled) {
_rig->setEnableInverseKinematics(isEnabled);
}
void MyAvatar::setEnableVerticalComfortMode(bool isEnabled) {
_enableVerticalComfortMode = isEnabled;
}
void MyAvatar::loadData() {
Settings settings;
settings.beginGroup("Avatar");
@ -1214,6 +1282,8 @@ void MyAvatar::rebuildCollisionShape() {
float scale = getUniformScale();
float radius = scale * _skeletonModel->getBoundingCapsuleRadius();
float height = scale * _skeletonModel->getBoundingCapsuleHeight() + 2.0f * radius;
const float CANONICAL_AVATAR_HEIGHT = 2.0f;
_canonicalScale = height / CANONICAL_AVATAR_HEIGHT;
glm::vec3 corner(-radius, -0.5f * height, -radius);
corner += scale * _skeletonModel->getBoundingCapsuleOffset();
glm::vec3 diagonal(2.0f * radius, height, 2.0f * radius);
@ -1271,10 +1341,14 @@ controller::Pose MyAvatar::getRightHandControllerPoseInAvatarFrame() const {
}
void MyAvatar::updateMotors() {
const float DEFAULT_MOTOR_TIMESCALE = 0.2f;
const float INVALID_MOTOR_TIMESCALE = 1.0e6f;
_characterController.clearMotors();
glm::quat motorRotation;
if (_motionBehaviors & AVATAR_MOTION_ACTION_MOTOR_ENABLED) {
if (_characterController.getState() == CharacterController::State::Hover) {
if (_characterController.getState() == CharacterController::State::Hover ||
_characterController.getCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS) {
motorRotation = getHead()->getCameraOrientation();
} else {
// non-hovering = walking: follow camera twist about vertical but not lift
@ -1282,14 +1356,18 @@ void MyAvatar::updateMotors() {
glm::quat liftRotation;
swingTwistDecomposition(getHead()->getCameraOrientation(), _worldUpDirection, liftRotation, motorRotation);
}
const float DEFAULT_MOTOR_TIMESCALE = 0.2f;
const float INVALID_MOTOR_TIMESCALE = 1.0e6f;
if (_isPushing || _isBraking || !_isBeingPushed) {
_characterController.addMotor(_actionMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE);
if (qApp->isHMDMode()) {
// OUTOFBODY_HACK: in HMDMode motors are applied differently: a "follow" motor is added
// during the CharacterController's substep
} else {
// _isBeingPushed must be true --> disable action motor by giving it a long timescale,
// otherwise it's attempt to "stand in in place" could defeat scripted motor/thrusts
_characterController.addMotor(_actionMotorVelocity, motorRotation, INVALID_MOTOR_TIMESCALE);
if (_isPushing || _isBraking || !_isBeingPushed) {
_characterController.addMotor(_actionMotorVelocity, motorRotation, DEFAULT_MOTOR_TIMESCALE, INVALID_MOTOR_TIMESCALE);
} else {
// _isBeingPushed must be true --> disable action motor by giving it a long timescale,
// otherwise it's attempt to "stand in in place" could defeat scripted motor/thrusts
_characterController.addMotor(_actionMotorVelocity, motorRotation, INVALID_MOTOR_TIMESCALE);
}
}
}
if (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED) {
@ -1301,16 +1379,19 @@ void MyAvatar::updateMotors() {
// world-frame
motorRotation = glm::quat();
}
_characterController.addMotor(_scriptedMotorVelocity, motorRotation, _scriptedMotorTimescale);
if (qApp->isHMDMode()) {
// OUTOFBODY_HACK: motors are applied differently in HMDMode
} else {
_characterController.addMotor(_scriptedMotorVelocity, motorRotation, _scriptedMotorTimescale);
}
}
// legacy support for 'MyAvatar::applyThrust()', which has always been implemented as a
// short-lived linearAcceleration
_characterController.setLinearAcceleration(_thrust);
_thrust = Vectors::ZERO;
_characterController.setLinearAcceleration(glm::vec3(0.0f, _thrust.y, 0.0f));
}
void MyAvatar::prepareForPhysicsSimulation() {
void MyAvatar::prepareForPhysicsSimulation(float deltaTime) {
relayDriveKeysToCharacterController();
updateMotors();
@ -1320,32 +1401,54 @@ void MyAvatar::prepareForPhysicsSimulation() {
qDebug() << "Warning: getParentVelocity failed" << getID();
parentVelocity = glm::vec3();
}
_characterController.handleChangedCollisionGroup();
_characterController.setParentVelocity(parentVelocity);
_characterController.setPositionAndOrientation(getPosition(), getOrientation());
glm::vec3 position = getPosition();
glm::quat orientation = getOrientation();
_characterController.setPositionAndOrientation(position, orientation);
if (qApp->isHMDMode()) {
_follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput());
// update the _bodySensorMatrix based on leaning behavior of the avatar.
_bodySensorMatrix = _follow.prePhysicsUpdate(*this, deriveBodyFromHMDSensor(), _bodySensorMatrix, hasDriveInput(), deltaTime);
// The avatar physics body always follows the _bodySensorMatrix.
glm::mat4 worldBodyMatrix = _sensorToWorldMatrix * _bodySensorMatrix;
getCharacterController()->setFollowParameters(worldBodyMatrix);
} else {
_follow.deactivate();
getCharacterController()->disableFollow();
}
_prePhysicsRoomPose = AnimPose(_sensorToWorldMatrix);
}
void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
// figure out how far the hips can move before they hit something
int hipsJoint = getJointIndex("Hips");
glm::vec3 hipsPosition; // rig-frame
// OUTOFBODY_HACK -- hardcoded maxHipsOffsetRadius (ultimately must exceed FollowHelper lateral/forward/back walk thresholds)
float maxHipsOffsetRadius = 3.0f * _characterController.getCapsuleRadius();
if (_rig->getJointPosition(hipsJoint, hipsPosition)) {
// OUTOFBODY_HACK -- flip PI about yAxis
hipsPosition.x *= -1.0f;
hipsPosition.z *= -1.0f;
maxHipsOffsetRadius = _characterController.measureMaxHipsOffsetRadius(hipsPosition, maxHipsOffsetRadius);
}
_rig->updateMaxHipsOffsetLength(maxHipsOffsetRadius, deltaTime);
glm::vec3 position = getPosition();
glm::quat orientation = getOrientation();
if (_characterController.isEnabledAndReady()) {
_characterController.getPositionAndOrientation(position, orientation);
}
nextAttitude(position, orientation);
_bodySensorMatrix = _follow.postPhysicsUpdate(*this, _bodySensorMatrix);
if (_characterController.isEnabledAndReady()) {
setVelocity(_characterController.getLinearVelocity() + _characterController.getFollowVelocity());
} else {
setVelocity(getVelocity() + _characterController.getFollowVelocity());
setVelocity(_characterController.getLinearVelocity());
}
_follow.postPhysicsUpdate(*this);
}
QString MyAvatar::getScriptedMotorFrame() const {
@ -1571,8 +1674,26 @@ void MyAvatar::postUpdate(float deltaTime) {
DebugDraw::getInstance().updateMyAvatarPos(getPosition());
DebugDraw::getInstance().updateMyAvatarRot(getOrientation());
AnimPose postUpdateRoomPose(_sensorToWorldMatrix);
if (_enableDebugDrawLeftFootTrace || _enableDebugDrawRightFootTrace) {
int boneIndex = _enableDebugDrawLeftFootTrace ? getJointIndex("LeftFoot") : getJointIndex("RightFoot");
const glm::vec4 RED(1.0f, 0.0f, 0.0f, 1.0f);
const glm::vec4 WHITE(1.0f, 1.0f, 1.0f, 1.0f);
const glm::vec4 TRANS(1.0f, 1.0f, 1.0f, 0.0f);
static bool colorBit = true;
colorBit = !colorBit;
glm::vec4 color = colorBit ? RED : WHITE;
_debugLineLoop[_debugLineLoopIndex] = DebugDrawVertex(getJointPosition(boneIndex), color);
_debugLineLoopIndex = (_debugLineLoopIndex + 1) % DEBUG_LINE_LOOP_SIZE;
_debugLineLoop[_debugLineLoopIndex] = DebugDrawVertex(getJointPosition(boneIndex), TRANS);
for (size_t prev = DEBUG_LINE_LOOP_SIZE - 1, next = 0; next < DEBUG_LINE_LOOP_SIZE; prev = next, next++) {
if (_debugLineLoop[prev].color.w > 0.0f) {
DebugDraw::getInstance().drawRay(_debugLineLoop[prev].pos, _debugLineLoop[next].pos, _debugLineLoop[prev].color);
}
}
}
AnimPose postUpdateRoomPose(_sensorToWorldMatrix);
updateHoldActions(_prePhysicsRoomPose, postUpdateRoomPose);
}
@ -1668,11 +1789,20 @@ void MyAvatar::updateOrientation(float deltaTime) {
// update body orientation by movement inputs
setOrientation(getOrientation() * glm::quat(glm::radians(glm::vec3(0.0f, totalBodyYaw, 0.0f))));
glm::quat deltaRotation = glm::quat(glm::radians(glm::vec3(0.0f, totalBodyYaw, 0.0f)));
setOrientation(getOrientation() * deltaRotation);
getHead()->setBasePitch(getHead()->getBasePitch() + _driveKeys[PITCH] * _pitchSpeed * deltaTime);
if (qApp->isHMDMode()) {
// rotate the sensorToWorldMatrix about the HMD!
glm::vec3 hmdOffset = extractTranslation(getHMDSensorMatrix());
_sensorToWorldMatrix = (_sensorToWorldMatrix *
createMatFromQuatAndPos(glm::quat(), hmdOffset) *
createMatFromQuatAndPos(deltaRotation, glm::vec3()) *
createMatFromQuatAndPos(glm::quat(), -hmdOffset));
glm::quat orientation = glm::quat_cast(getSensorToWorldMatrix()) * getHMDSensorOrientation();
glm::quat bodyOrientation = getWorldBodyOrientation();
glm::quat localOrientation = glm::inverse(bodyOrientation) * orientation;
@ -1689,6 +1819,7 @@ void MyAvatar::updateOrientation(float deltaTime) {
}
void MyAvatar::updateActionMotor(float deltaTime) {
bool thrustIsPushing = (glm::length2(_thrust) > EPSILON);
bool scriptedMotorIsPushing = (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED)
&& _scriptedMotorTimescale < MAX_CHARACTER_MOTOR_TIMESCALE;
@ -1728,11 +1859,11 @@ void MyAvatar::updateActionMotor(float deltaTime) {
if (state == CharacterController::State::Hover) {
// we're flying --> complex acceleration curve that builds on top of current motor speed and caps at some max speed
float motorSpeed = glm::length(_actionMotorVelocity);
float motorSpeed = glm::length(glm::vec3(_actionMotorVelocity.x, _actionMotorVelocity.y, _actionMotorVelocity.z));
float finalMaxMotorSpeed = getUniformScale() * MAX_ACTION_MOTOR_SPEED;
float speedGrowthTimescale = 2.0f;
float speedIncreaseFactor = 1.8f;
motorSpeed *= 1.0f + glm::clamp(deltaTime / speedGrowthTimescale , 0.0f, 1.0f) * speedIncreaseFactor;
motorSpeed *= 1.0f + glm::clamp(deltaTime / speedGrowthTimescale, 0.0f, 1.0f) * speedIncreaseFactor;
const float maxBoostSpeed = getUniformScale() * MAX_BOOST_SPEED;
if (_isPushing) {
@ -1755,6 +1886,21 @@ void MyAvatar::updateActionMotor(float deltaTime) {
_boomLength = glm::clamp<float>(_boomLength, ZOOM_MIN, ZOOM_MAX);
}
void MyAvatar::applyVelocityToSensorToWorldMatrix(const glm::vec3& velocity, float deltaTime) {
glm::vec3 newVelocity = velocity;
if (_characterController.getState() != CharacterController::State::Hover) {
newVelocity -= glm::dot(newVelocity, _worldUpDirection);
}
float speed2 = glm::length2(newVelocity);
if (speed2 > MIN_AVATAR_SPEED_SQUARED) {
glm::vec3 position = extractTranslation(_sensorToWorldMatrix) + deltaTime * newVelocity;
// update the position column of matrix
glm::mat4 newSensorToWorldMatrix = _sensorToWorldMatrix;
newSensorToWorldMatrix[3] = glm::vec4(position, 1.0f);
setSensorToWorldMatrix(newSensorToWorldMatrix);
}
}
void MyAvatar::updatePosition(float deltaTime) {
if (_motionBehaviors & AVATAR_MOTION_ACTION_MOTOR_ENABLED) {
updateActionMotor(deltaTime);
@ -1775,11 +1921,61 @@ void MyAvatar::updatePosition(float deltaTime) {
measureMotionDerivatives(deltaTime);
_moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED;
} else {
// physics physics simulation updated elsewhere
float speed2 = glm::length2(velocity);
_moving = speed2 > MOVING_SPEED_THRESHOLD_SQUARED;
if (_moving) {
// scan for walkability
glm::vec3 position = getPosition();
MyCharacterController::RayShotgunResult result;
glm::vec3 step = deltaTime * (getRotation() * _actionMotorVelocity);
_characterController.testRayShotgun(position, step, result);
_characterController.setStepUpEnabled(result.walkable);
}
if (qApp->isHMDMode()) {
glm::quat motorRotation;
glm::vec3 worldVelocity = glm::vec3(0.0f);
if (_motionBehaviors & AVATAR_MOTION_ACTION_MOTOR_ENABLED) {
if (_characterController.getState() == CharacterController::State::Hover ||
_characterController.getCollisionGroup() == BULLET_COLLISION_GROUP_COLLISIONLESS) {
motorRotation = glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix());
} else {
glm::quat liftRotation;
swingTwistDecomposition(glmExtractRotation(_sensorToWorldMatrix * getHMDSensorMatrix()), _worldUpDirection, liftRotation, motorRotation);
}
worldVelocity = motorRotation * _actionMotorVelocity;
}
if (_motionBehaviors & AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED) {
if (_scriptedMotorFrame == SCRIPTED_MOTOR_CAMERA_FRAME) {
motorRotation = getHead()->getCameraOrientation() * glm::angleAxis(PI, Vectors::UNIT_Y);
} else if (_scriptedMotorFrame == SCRIPTED_MOTOR_AVATAR_FRAME) {
motorRotation = getOrientation() * glm::angleAxis(PI, Vectors::UNIT_Y);
} else {
// world-frame
motorRotation = glm::quat();
}
worldVelocity += motorRotation * _scriptedMotorVelocity;
}
// OUTOFBODY_HACK: apply scaling factor to _thrust, to get the same behavior as an periodically applied motor.
const float THRUST_DAMPING_FACTOR = 0.25f;
worldVelocity += THRUST_DAMPING_FACTOR * _thrust;
// apply velocity directly to _sensorToWorldMatrix.
if (glm::length2(worldVelocity) > FLT_EPSILON) {
glm::mat4 worldBodyMatrix = _sensorToWorldMatrix * _bodySensorMatrix;
glm::vec3 position = extractTranslation(worldBodyMatrix);
glm::vec3 step = deltaTime * worldVelocity;
glm::vec3 newVelocity = _characterController.computeHMDStep(position, step) / deltaTime;
applyVelocityToSensorToWorldMatrix(newVelocity, deltaTime);
}
}
}
_thrust = Vectors::ZERO;
// capture the head rotation, in sensor space, when the user first indicates they would like to move/fly.
if (!_hoverReferenceCameraFacingIsCaptured && (fabs(_driveKeys[TRANSLATE_Z]) > 0.1f || fabs(_driveKeys[TRANSLATE_X]) > 0.1f)) {
_hoverReferenceCameraFacingIsCaptured = true;
@ -1832,6 +2028,10 @@ bool findAvatarAvatarPenetration(const glm::vec3 positionA, float radiusA, float
return false;
}
glm::vec3 MyAvatar::getPreActionVelocity() const {
return _characterController.getPreActionLinearVelocity();
}
// There can be a separation between the _targetScale and the actual scale of the rendered avatar in a domain.
// When the avatar enters a domain where their target scale is not allowed according to the min/max
// we do not change their saved target scale. Instead, we use getDomainLimitedScale() to render the avatar
@ -2015,13 +2215,17 @@ void MyAvatar::updateMotionBehaviorFromMenu() {
_motionBehaviors &= ~AVATAR_MOTION_SCRIPTED_MOTOR_ENABLED;
}
setCharacterControllerEnabled(menu->isOptionChecked(MenuOption::EnableCharacterController));
// KINEMATIC_CONTROLLER_HACK
bool moveKinematically = menu->isOptionChecked(MenuOption::MoveKinematically);
_characterController.setMoveKinematically(moveKinematically);
setAvatarCollisionsEnabled(menu->isOptionChecked(MenuOption::EnableAvatarCollisions));
}
void MyAvatar::setCharacterControllerEnabled(bool enabled) {
void MyAvatar::setAvatarCollisionsEnabled(bool enabled) {
if (QThread::currentThread() != thread()) {
QMetaObject::invokeMethod(this, "setCharacterControllerEnabled", Q_ARG(bool, enabled));
QMetaObject::invokeMethod(this, "setAvatarCollisionsEnabled", Q_ARG(bool, enabled));
return;
}
@ -2033,11 +2237,12 @@ void MyAvatar::setCharacterControllerEnabled(bool enabled) {
ghostingAllowed = zone->getGhostingAllowed();
}
}
_characterController.setEnabled(ghostingAllowed ? enabled : true);
int16_t group = enabled || !ghostingAllowed ? BULLET_COLLISION_GROUP_MY_AVATAR : BULLET_COLLISION_GROUP_COLLISIONLESS;
_characterController.setCollisionGroup(group);
}
bool MyAvatar::getCharacterControllerEnabled() {
return _characterController.isEnabled();
bool MyAvatar::getAvatarCollisionsEnabled() {
return _characterController.getCollisionGroup() != BULLET_COLLISION_GROUP_COLLISIONLESS;
}
void MyAvatar::clearDriveKeys() {
@ -2125,6 +2330,10 @@ glm::quat MyAvatar::getOrientationForAudio() {
return quat();
}
bool MyAvatar::isOutOfBody() const {
return _follow._isOutOfBody;
}
void MyAvatar::setAudioListenerMode(AudioListenerMode audioListenerMode) {
if (_audioListenerMode != audioListenerMode) {
_audioListenerMode = audioListenerMode;
@ -2145,72 +2354,52 @@ void MyAvatar::lateUpdatePalms() {
Avatar::updatePalms();
}
static const float FOLLOW_TIME = 0.5f;
MyAvatar::FollowHelper::FollowHelper() {
deactivate();
}
void MyAvatar::FollowHelper::deactivate() {
for (int i = 0; i < NumFollowTypes; i++) {
deactivate((FollowType)i);
}
_activeBits = 0;
}
void MyAvatar::FollowHelper::deactivate(FollowType type) {
assert(type >= 0 && type < NumFollowTypes);
_timeRemaining[(int)type] = 0.0f;
_activeBits &= ~(uint8_t)(0x01 << (int)type);
}
void MyAvatar::FollowHelper::activate(FollowType type) {
assert(type >= 0 && type < NumFollowTypes);
// TODO: Perhaps, the follow time should be proportional to the displacement.
_timeRemaining[(int)type] = FOLLOW_TIME;
_activeBits |= (uint8_t)(0x01 << (int)type);
}
bool MyAvatar::FollowHelper::isActive(FollowType type) const {
assert(type >= 0 && type < NumFollowTypes);
return _timeRemaining[(int)type] > 0.0f;
return (bool)(_activeBits & (uint8_t)(0x01 << (int)type));
}
bool MyAvatar::FollowHelper::isActive() const {
for (int i = 0; i < NumFollowTypes; i++) {
if (isActive((FollowType)i)) {
return true;
}
}
return false;
return (bool)_activeBits;
}
float MyAvatar::FollowHelper::getMaxTimeRemaining() const {
float max = 0.0f;
for (int i = 0; i < NumFollowTypes; i++) {
if (_timeRemaining[i] > max) {
max = _timeRemaining[i];
}
}
return max;
}
void MyAvatar::FollowHelper::decrementTimeRemaining(float dt) {
for (int i = 0; i < NumFollowTypes; i++) {
_timeRemaining[i] -= dt;
}
}
bool MyAvatar::FollowHelper::shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
void MyAvatar::FollowHelper::updateRotationActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) {
auto cameraMode = qApp->getCamera()->getMode();
if (cameraMode == CAMERA_MODE_THIRD_PERSON) {
return false;
deactivate(Rotation);
} else {
const float FOLLOW_ROTATION_THRESHOLD = cosf(PI / 6.0f); // 30 degrees
const float STOP_FOLLOW_ROTATION_THRESHOLD = cosf(PI / 180.0f); // 1 degree
glm::vec2 bodyFacing = getFacingDir2D(currentBodyMatrix);
return glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD;
if (isActive(Rotation)) {
if (glm::dot(myAvatar.getHMDSensorFacing(), bodyFacing) > STOP_FOLLOW_ROTATION_THRESHOLD) {
deactivate(Rotation);
}
} else if (glm::dot(myAvatar.getHMDSensorFacingMovingAverage(), bodyFacing) < FOLLOW_ROTATION_THRESHOLD) {
activate(Rotation);
}
}
}
bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
void MyAvatar::FollowHelper::updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) {
// -z axis of currentBodyMatrix in world space.
glm::vec3 forward = glm::normalize(glm::vec3(-currentBodyMatrix[0][2], -currentBodyMatrix[1][2], -currentBodyMatrix[2][2]));
@ -2219,88 +2408,86 @@ bool MyAvatar::FollowHelper::shouldActivateHorizontal(const MyAvatar& myAvatar,
glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix);
float forwardLeanAmount = glm::dot(forward, offset);
float lateralLeanAmount = glm::dot(right, offset);
float lateralLeanAmount = fabsf(glm::dot(right, offset));
const float MAX_LATERAL_LEAN = 0.3f;
const float MAX_FORWARD_LEAN = 0.15f;
const float MAX_BACKWARD_LEAN = 0.1f;
const float MIN_LEAN = 0.02f;
if (forwardLeanAmount > 0 && forwardLeanAmount > MAX_FORWARD_LEAN) {
return true;
} else if (forwardLeanAmount < 0 && forwardLeanAmount < -MAX_BACKWARD_LEAN) {
return true;
}
return fabs(lateralLeanAmount) > MAX_LATERAL_LEAN;
}
bool MyAvatar::FollowHelper::shouldActivateVertical(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const {
const float CYLINDER_TOP = 0.1f;
const float CYLINDER_BOTTOM = -1.5f;
glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix);
return (offset.y > CYLINDER_TOP) || (offset.y < CYLINDER_BOTTOM);
}
void MyAvatar::FollowHelper::prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput) {
_desiredBodyMatrix = desiredBodyMatrix;
if (myAvatar.getHMDLeanRecenterEnabled()) {
if (!isActive(Rotation) && shouldActivateRotation(myAvatar, desiredBodyMatrix, currentBodyMatrix)) {
activate(Rotation);
if (isActive(Horizontal)) {
if (fabsf(forwardLeanAmount) < MIN_LEAN && lateralLeanAmount < MIN_LEAN) {
deactivate(Horizontal);
}
if (!isActive(Horizontal) && shouldActivateHorizontal(myAvatar, desiredBodyMatrix, currentBodyMatrix)) {
} else {
if (forwardLeanAmount > MAX_FORWARD_LEAN ||
forwardLeanAmount < -MAX_BACKWARD_LEAN ||
lateralLeanAmount > MAX_LATERAL_LEAN) {
activate(Horizontal);
}
if (!isActive(Vertical) && (shouldActivateVertical(myAvatar, desiredBodyMatrix, currentBodyMatrix) || hasDriveInput)) {
activate(Vertical);
}
}
glm::mat4 desiredWorldMatrix = myAvatar.getSensorToWorldMatrix() * _desiredBodyMatrix;
glm::mat4 currentWorldMatrix = myAvatar.getSensorToWorldMatrix() * currentBodyMatrix;
AnimPose followWorldPose(currentWorldMatrix);
if (isActive(Rotation)) {
followWorldPose.rot = glmExtractRotation(desiredWorldMatrix);
}
if (isActive(Horizontal)) {
glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix);
followWorldPose.trans.x = desiredTranslation.x;
followWorldPose.trans.z = desiredTranslation.z;
}
if (isActive(Vertical)) {
glm::vec3 desiredTranslation = extractTranslation(desiredWorldMatrix);
followWorldPose.trans.y = desiredTranslation.y;
}
myAvatar.getCharacterController()->setFollowParameters(followWorldPose, getMaxTimeRemaining());
}
glm::mat4 MyAvatar::FollowHelper::postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix) {
if (isActive()) {
float dt = myAvatar.getCharacterController()->getFollowTime();
decrementTimeRemaining(dt);
void MyAvatar::FollowHelper::updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) {
const float CYLINDER_TOP = 0.1f;
const float CYLINDER_BOTTOM = -1.5f;
const float MIN_VERTICAL_OFFSET = 0.02f;
// apply follow displacement to the body matrix.
glm::vec3 worldLinearDisplacement = myAvatar.getCharacterController()->getFollowLinearDisplacement();
glm::quat worldAngularDisplacement = myAvatar.getCharacterController()->getFollowAngularDisplacement();
glm::quat sensorToWorld = glmExtractRotation(myAvatar.getSensorToWorldMatrix());
glm::quat worldToSensor = glm::inverse(sensorToWorld);
glm::vec3 offset = extractTranslation(desiredBodyMatrix) - extractTranslation(currentBodyMatrix);
if (isActive(Vertical)) {
if (fabsf(offset.y) < MIN_VERTICAL_OFFSET) {
deactivate(Vertical);
}
} else if (offset.y > CYLINDER_TOP || offset.y < CYLINDER_BOTTOM) {
activate(Vertical);
}
}
glm::vec3 sensorLinearDisplacement = worldToSensor * worldLinearDisplacement;
glm::quat sensorAngularDisplacement = worldToSensor * worldAngularDisplacement * sensorToWorld;
glm::mat4 MyAvatar::FollowHelper::prePhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput, float deltaTime) {
if (myAvatar.getHMDLeanRecenterEnabled()) {
updateRotationActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix);
updateHorizontalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix);
updateVerticalActivation(myAvatar, desiredBodyMatrix, currentBodyMatrix);
glm::mat4 newBodyMat = createMatFromQuatAndPos(sensorAngularDisplacement * glmExtractRotation(currentBodyMatrix),
sensorLinearDisplacement + extractTranslation(currentBodyMatrix));
return newBodyMat;
AnimPose currentBodyPose(currentBodyMatrix);
AnimPose desiredBodyPose(desiredBodyMatrix);
AnimPose followBodyPose(currentBodyMatrix);
if (isActive(Rotation) || hasDriveInput) {
followBodyPose.rot = desiredBodyPose.rot;
}
if (isActive(Horizontal) || hasDriveInput) {
followBodyPose.trans.x = desiredBodyPose.trans.x;
followBodyPose.trans.z = desiredBodyPose.trans.z;
}
if (isActive(Vertical) || hasDriveInput) {
followBodyPose.trans.y = desiredBodyPose.trans.y;
}
if (isActive() || hasDriveInput) {
const float TIMESCALE = 0.2f;
const float tau = glm::clamp(deltaTime / TIMESCALE, 0.0f, 1.0f);
AnimPose newBodyPose;
blend(1, &currentBodyPose, &followBodyPose, tau, &newBodyPose);
return (glm::mat4)newBodyPose;
} else {
return currentBodyMatrix;
}
} else {
deactivate();
return currentBodyMatrix;
}
}
void MyAvatar::FollowHelper::postPhysicsUpdate(MyAvatar& myAvatar) {
glm::mat4 worldHMDMat = myAvatar.getSensorToWorldMatrix() * myAvatar.getHMDSensorMatrix();
glm::vec3 worldHMDPosition = extractTranslation(worldHMDMat);
glm::vec3 capsuleStart = myAvatar.getPosition() + Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
glm::vec3 capsuleEnd = myAvatar.getPosition() - Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
_isOutOfBody = !pointIsInsideCapsule(worldHMDPosition, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS);
_outOfBodyDistance = distanceFromCapsule(worldHMDPosition, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS);
}
float MyAvatar::getAccelerationEnergy() {
glm::vec3 velocity = getVelocity();
int changeInVelocity = abs(velocity.length() - priorVelocity.length());

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

@ -56,6 +56,8 @@ enum AudioListenerMode {
};
Q_DECLARE_METATYPE(AudioListenerMode);
const size_t DEBUG_LINE_LOOP_SIZE = 500;
class MyAvatar : public Avatar {
Q_OBJECT
Q_PROPERTY(bool shouldRenderLocally READ getShouldRenderLocally WRITE setShouldRenderLocally)
@ -84,7 +86,7 @@ class MyAvatar : public Avatar {
Q_PROPERTY(float energy READ getEnergy WRITE setEnergy)
Q_PROPERTY(bool hmdLeanRecenterEnabled READ getHMDLeanRecenterEnabled WRITE setHMDLeanRecenterEnabled)
Q_PROPERTY(bool characterControllerEnabled READ getCharacterControllerEnabled WRITE setCharacterControllerEnabled)
Q_PROPERTY(bool avatarCollisionsEnabled READ getAvatarCollisionsEnabled WRITE setAvatarCollisionsEnabled)
public:
explicit MyAvatar(RigPointer rig);
@ -108,6 +110,7 @@ public:
const glm::mat4& getHMDSensorMatrix() const { return _hmdSensorMatrix; }
const glm::vec3& getHMDSensorPosition() const { return _hmdSensorPosition; }
const glm::quat& getHMDSensorOrientation() const { return _hmdSensorOrientation; }
const glm::vec2& getHMDSensorFacing() const { return _hmdSensorFacing; }
const glm::vec2& getHMDSensorFacingMovingAverage() const { return _hmdSensorFacingMovingAverage; }
Q_INVOKABLE void setOrientationVar(const QVariant& newOrientationVar);
@ -125,7 +128,14 @@ public:
// 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.
void updateSensorToWorldMatrix();
enum class SensorToWorldUpdateMode {
Full = 0,
Vertical,
VerticalComfort
};
void updateSensorToWorldMatrix(SensorToWorldUpdateMode mode = SensorToWorldUpdateMode::Full);
void setSensorToWorldMatrix(const glm::mat4& sensorToWorld);
void setRealWorldFieldOfView(float realWorldFov) { _realWorldFieldOfView.set(realWorldFov); }
@ -227,7 +237,7 @@ public:
const MyCharacterController* getCharacterController() const { return &_characterController; }
void updateMotors();
void prepareForPhysicsSimulation();
void prepareForPhysicsSimulation(float deltaTime);
void harvestResultsFromPhysicsSimulation(float deltaTime);
const QString& getCollisionSoundURL() { return _collisionSoundURL; }
@ -272,12 +282,14 @@ public:
bool hasDriveInput() const;
Q_INVOKABLE void setCharacterControllerEnabled(bool enabled);
Q_INVOKABLE bool getCharacterControllerEnabled();
Q_INVOKABLE void setAvatarCollisionsEnabled(bool enabled);
Q_INVOKABLE bool getAvatarCollisionsEnabled();
virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override;
virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override;
glm::vec3 getPreActionVelocity() const;
void addHoldAction(AvatarActionHold* holdAction); // thread-safe
void removeHoldAction(AvatarActionHold* holdAction); // thread-safe
void updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose& postUpdatePose);
@ -302,15 +314,19 @@ public slots:
Q_INVOKABLE void updateMotionBehaviorFromMenu();
void setEnableDebugDrawLeftFootTrace(bool isEnabled);
void setEnableDebugDrawRightFootTrace(bool isEnabled);
void setEnableDebugDrawDefaultPose(bool isEnabled);
void setEnableDebugDrawAnimPose(bool isEnabled);
void setEnableDebugDrawPosition(bool isEnabled);
void setEnableDebugDrawHandControllers(bool isEnabled);
void setEnableDebugDrawSensorToWorldMatrix(bool isEnabled);
void setEnableDebugDrawIKTargets(bool isEnabled);
bool getEnableMeshVisible() const { return _skeletonModel->isVisible(); }
void setEnableMeshVisible(bool isEnabled);
void setUseAnimPreAndPostRotations(bool isEnabled);
void setEnableInverseKinematics(bool isEnabled);
void setEnableVerticalComfortMode(bool isEnabled);
QUrl getAnimGraphOverrideUrl() const; // thread-safe
void setAnimGraphOverrideUrl(QUrl value); // thread-safe
@ -320,6 +336,8 @@ public slots:
glm::vec3 getPositionForAudio();
glm::quat getOrientationForAudio();
bool isOutOfBody() const;
signals:
void audioListenerModeChanged();
void transformChanged();
@ -372,6 +390,8 @@ private:
virtual void updatePalms() override {}
void lateUpdatePalms();
void applyVelocityToSensorToWorldMatrix(const glm::vec3& velocity, float deltaTime);
void clampTargetScaleToDomainLimits();
void clampScaleChangeToDomainLimits(float desiredScale);
glm::mat4 computeCameraRelativeHandControllerMatrix(const glm::mat4& controllerSensorMatrix) const;
@ -452,8 +472,9 @@ private:
Vertical,
NumFollowTypes
};
glm::mat4 _desiredBodyMatrix;
float _timeRemaining[NumFollowTypes];
uint8_t _activeBits { 0 };
bool _isOutOfBody { false };
float _outOfBodyDistance { 0.0f };
void deactivate();
void deactivate(FollowType type);
@ -461,13 +482,11 @@ private:
void activate(FollowType type);
bool isActive() const;
bool isActive(FollowType followType) const;
float getMaxTimeRemaining() const;
void decrementTimeRemaining(float dt);
bool shouldActivateRotation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const;
bool shouldActivateVertical(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const;
bool shouldActivateHorizontal(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix) const;
void prePhysicsUpdate(MyAvatar& myAvatar, const glm::mat4& bodySensorMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput);
glm::mat4 postPhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& currentBodyMatrix);
void updateRotationActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix);
void updateHorizontalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix);
void updateVerticalActivation(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix);
glm::mat4 prePhysicsUpdate(const MyAvatar& myAvatar, const glm::mat4& desiredBodyMatrix, const glm::mat4& currentBodyMatrix, bool hasDriveInput, float deltaTime);
void postPhysicsUpdate(MyAvatar& myAvatar);
};
FollowHelper _follow;
@ -479,10 +498,14 @@ private:
RigPointer _rig;
bool _prevShouldDrawHead;
bool _enableDebugDrawLeftFootTrace { false };
bool _enableDebugDrawRightFootTrace { false };
bool _enableDebugDrawDefaultPose { false };
bool _enableDebugDrawAnimPose { false };
bool _enableDebugDrawHandControllers { false };
bool _enableDebugDrawSensorToWorldMatrix { false };
bool _enableDebugDrawIKTargets { false };
bool _enableVerticalComfortMode { false };
AudioListenerMode _audioListenerMode;
glm::vec3 _customListenPosition;
@ -498,6 +521,8 @@ private:
ThreadSafeValueCache<controller::Pose> _rightHandControllerPoseInSensorFrameCache { controller::Pose() };
bool _hmdLeanRecenterEnabled = true;
bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK
float _canonicalScale { 1.0f };
AnimPose _prePhysicsRoomPose;
std::mutex _holdActionsMutex;
@ -515,6 +540,18 @@ private:
float getEnergy();
void setEnergy(float value);
bool didTeleport();
struct DebugDrawVertex {
DebugDrawVertex() : pos(), color() {}
DebugDrawVertex(const glm::vec3& posIn, const glm::vec4& colorIn) : pos(posIn), color(colorIn) {}
glm::vec3 pos;
glm::vec4 color;
};
DebugDrawVertex _debugLineLoop[DEBUG_LINE_LOOP_SIZE];
size_t _debugLineLoopIndex { 0 };
bool _handControllerShow { false };
float _handControllerShowTimer { 0.0f };
};
QScriptValue audioListenModeToScriptValue(QScriptEngine* engine, const AudioListenerMode& audioListenerMode);

443
interface/src/avatar/MyCharacterController.cpp Normal file → Executable file
View file

@ -15,11 +15,15 @@
#include "MyAvatar.h"
// TODO: improve walking up steps
// TODO: make avatars able to walk up and down steps/slopes
// TODO: make avatars stand on steep slope
// TODO: make avatars not snag on low ceilings
void MyCharacterController::RayShotgunResult::reset() {
hitFraction = 1.0f;
walkable = true;
}
MyCharacterController::MyCharacterController(MyAvatar* avatar) {
assert(avatar);
@ -30,37 +34,33 @@ MyCharacterController::MyCharacterController(MyAvatar* avatar) {
MyCharacterController::~MyCharacterController() {
}
void MyCharacterController::setDynamicsWorld(btDynamicsWorld* world) {
CharacterController::setDynamicsWorld(world);
if (world) {
initRayShotgun(world);
}
}
void MyCharacterController::updateShapeIfNecessary() {
if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
_pendingFlags &= ~PENDING_FLAG_UPDATE_SHAPE;
// compute new dimensions from avatar's bounding box
float x = _boxScale.x;
float z = _boxScale.z;
_radius = 0.5f * sqrtf(0.5f * (x * x + z * z));
_halfHeight = 0.5f * _boxScale.y - _radius;
float MIN_HALF_HEIGHT = 0.1f;
if (_halfHeight < MIN_HALF_HEIGHT) {
_halfHeight = MIN_HALF_HEIGHT;
}
// NOTE: _shapeLocalOffset is already computed
if (_radius > 0.0f) {
// create RigidBody if it doesn't exist
if (!_rigidBody) {
btCollisionShape* shape = computeShape();
// HACK: use some simple mass property defaults for now
const float DEFAULT_AVATAR_MASS = 100.0f;
const btScalar DEFAULT_AVATAR_MASS = 100.0f;
const btVector3 DEFAULT_AVATAR_INERTIA_TENSOR(30.0f, 8.0f, 30.0f);
btCollisionShape* shape = new btCapsuleShape(_radius, 2.0f * _halfHeight);
_rigidBody = new btRigidBody(DEFAULT_AVATAR_MASS, nullptr, shape, DEFAULT_AVATAR_INERTIA_TENSOR);
} else {
btCollisionShape* shape = _rigidBody->getCollisionShape();
if (shape) {
delete shape;
}
shape = new btCapsuleShape(_radius, 2.0f * _halfHeight);
shape = computeShape();
_rigidBody->setCollisionShape(shape);
}
@ -72,12 +72,419 @@ void MyCharacterController::updateShapeIfNecessary() {
if (_state == State::Hover) {
_rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
} else {
_rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
_rigidBody->setGravity(_gravity * _currentUp);
}
// KINEMATIC_CONTROLLER_HACK
if (_moveKinematically) {
_rigidBody->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
} else {
_rigidBody->setCollisionFlags(_rigidBody->getCollisionFlags() &
~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT));
}
//_rigidBody->setCollisionFlags(btCollisionObject::CF_CHARACTER_OBJECT);
} else {
// TODO: handle this failure case
}
}
}
bool MyCharacterController::testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result) {
btVector3 rayDirection = glmToBullet(step);
btScalar stepLength = rayDirection.length();
if (stepLength < FLT_EPSILON) {
return false;
}
rayDirection /= stepLength;
// get _ghost ready for ray traces
btTransform transform = _rigidBody->getWorldTransform();
btVector3 newPosition = glmToBullet(position);
transform.setOrigin(newPosition);
_ghost.setWorldTransform(transform);
btMatrix3x3 rotation = transform.getBasis();
_ghost.refreshOverlappingPairCache();
CharacterRayResult rayResult(&_ghost);
CharacterRayResult closestRayResult(&_ghost);
btVector3 rayStart;
btVector3 rayEnd;
// compute rotation that will orient local ray start points to face step direction
btVector3 forward = rotation * btVector3(0.0f, 0.0f, -1.0f);
btVector3 adjustedDirection = rayDirection - rayDirection.dot(_currentUp) * _currentUp;
btVector3 axis = forward.cross(adjustedDirection);
btScalar lengthAxis = axis.length();
if (lengthAxis > FLT_EPSILON) {
// we're walking sideways
btScalar angle = acosf(lengthAxis / adjustedDirection.length());
if (rayDirection.dot(forward) < 0.0f) {
angle = PI - angle;
}
axis /= lengthAxis;
rotation = btMatrix3x3(btQuaternion(axis, angle)) * rotation;
} else if (rayDirection.dot(forward) < 0.0f) {
// we're walking backwards
rotation = btMatrix3x3(btQuaternion(_currentUp, PI)) * rotation;
}
// scan the top
// NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on.
// The approximate extra distance can be derived with trigonometry.
//
// minimumForward = [ (maxStepHeight + radius / cosTheta - radius) * (cosTheta / sinTheta) - radius ]
//
// where: theta = max angle between floor normal and vertical
//
// if stepLength is not long enough we can add the difference.
//
btScalar cosTheta = _minFloorNormalDotUp;
btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
const btScalar MIN_FORWARD_SLOP = 0.12f; // HACK: not sure why this is necessary to detect steepest walkable slope
btScalar forwardSlop = (_maxStepHeight + _radius / cosTheta - _radius) * (cosTheta / sinTheta) - (_radius + stepLength) + MIN_FORWARD_SLOP;
if (forwardSlop < 0.0f) {
// BIG step, no slop necessary
forwardSlop = 0.0f;
}
const btScalar backSlop = 0.04f;
for (int32_t i = 0; i < _topPoints.size(); ++i) {
rayStart = newPosition + rotation * _topPoints[i] - backSlop * rayDirection;
rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) {
closestRayResult = rayResult;
}
if (result.walkable) {
if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) {
result.walkable = false;
// the top scan wasn't walkable so don't bother scanning the bottom
// remove both forwardSlop and backSlop
result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength);
return result.hitFraction < 1.0f;
}
}
}
}
if (_state == State::Hover) {
// scan the bottom just like the top
for (int32_t i = 0; i < _bottomPoints.size(); ++i) {
rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection;
rayEnd = rayStart + (backSlop + stepLength + forwardSlop) * rayDirection;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) {
closestRayResult = rayResult;
}
if (result.walkable) {
if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) {
result.walkable = false;
// the bottom scan wasn't walkable
// remove both forwardSlop and backSlop
result.hitFraction = glm::min(1.0f, (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop) - backSlop) / stepLength);
return result.hitFraction < 1.0f;
}
}
}
}
} else {
// scan the bottom looking for nearest step point
// remove forwardSlop
result.hitFraction = (closestRayResult.m_closestHitFraction * (backSlop + stepLength + forwardSlop)) / (backSlop + stepLength);
for (int32_t i = 0; i < _bottomPoints.size(); ++i) {
rayStart = newPosition + rotation * _bottomPoints[i] - backSlop * rayDirection;
rayEnd = rayStart + (backSlop + stepLength) * rayDirection;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
if (rayResult.m_closestHitFraction < closestRayResult.m_closestHitFraction) {
closestRayResult = rayResult;
}
}
}
// remove backSlop
// NOTE: backSlop removal can produce a NEGATIVE hitFraction!
// which means the shape is actually in interpenetration
result.hitFraction = ((closestRayResult.m_closestHitFraction * (backSlop + stepLength)) - backSlop) / stepLength;
}
return result.hitFraction < 1.0f;
}
glm::vec3 MyCharacterController::computeHMDStep(const glm::vec3& position, const glm::vec3& step) {
btVector3 stepDirection = glmToBullet(step);
btScalar stepLength = stepDirection.length();
if (stepLength < FLT_EPSILON) {
return glm::vec3(0.0f);
}
stepDirection /= stepLength;
// get _ghost ready for ray traces
btTransform transform = _rigidBody->getWorldTransform();
btVector3 newPosition = glmToBullet(position);
transform.setOrigin(newPosition);
btMatrix3x3 rotation = transform.getBasis();
_ghost.setWorldTransform(transform);
_ghost.refreshOverlappingPairCache();
// compute rotation that will orient local ray start points to face stepDirection
btVector3 forward = rotation * btVector3(0.0f, 0.0f, -1.0f);
btVector3 horizontalDirection = stepDirection - stepDirection.dot(_currentUp) * _currentUp;
btVector3 axis = forward.cross(horizontalDirection);
btScalar lengthAxis = axis.length();
if (lengthAxis > FLT_EPSILON) {
// non-zero sideways component
btScalar angle = asinf(lengthAxis / horizontalDirection.length());
if (stepDirection.dot(forward) < 0.0f) {
angle = PI - angle;
}
axis /= lengthAxis;
rotation = btMatrix3x3(btQuaternion(axis, angle)) * rotation;
} else if (stepDirection.dot(forward) < 0.0f) {
// backwards
rotation = btMatrix3x3(btQuaternion(_currentUp, PI)) * rotation;
}
CharacterRayResult rayResult(&_ghost);
btVector3 rayStart;
btVector3 rayEnd;
btVector3 penetration = btVector3(0.0f, 0.0f, 0.0f);
int32_t numPenetrations = 0;
{ // first we scan straight out from capsule center to see if we're stuck on anything
btScalar forwardRatio = 0.5f;
btScalar backRatio = 0.25f;
btVector3 radial;
bool stuck = false;
for (int32_t i = 0; i < _topPoints.size(); ++i) {
rayStart = rotation * _topPoints[i];
radial = rayStart - rayStart.dot(_currentUp) * _currentUp;
rayEnd = newPosition + rayStart + forwardRatio * radial;
rayStart += newPosition - backRatio * radial;
// reset rayResult for next test
rayResult.m_closestHitFraction = 1.0f;
rayResult.m_collisionObject = nullptr;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
btScalar totalRatio = backRatio + forwardRatio;
btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * totalRatio - backRatio) / forwardRatio;
if (adjustedHitFraction < 0.0f) {
penetration += adjustedHitFraction * radial;
++numPenetrations;
} else {
stuck = true;
}
}
}
if (numPenetrations > 0) {
if (numPenetrations > 1) {
penetration /= (btScalar)numPenetrations;
}
return bulletToGLM(penetration);
} else if (stuck) {
return glm::vec3(0.0f);
}
}
// if we get here then we're not stuck pushing into any surface
// so now we scan to see if the way before us is "walkable"
// scan the top
// NOTE: if we scan an extra distance forward we can detect flat surfaces that are too steep to walk on.
// The approximate extra distance can be derived with trigonometry.
//
// minimumForward = [ (maxStepHeight + radius / cosTheta - radius) * (cosTheta / sinTheta) - radius ]
//
// where: theta = max angle between floor normal and vertical
//
// if stepLength is not long enough we can add the difference.
//
btScalar cosTheta = _minFloorNormalDotUp;
btScalar sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
const btScalar MIN_FORWARD_SLOP = 0.10f; // HACK: not sure why this is necessary to detect steepest walkable slope
btScalar forwardSlop = (_maxStepHeight + _radius / cosTheta - _radius) * (cosTheta / sinTheta) - (_radius + stepLength) + MIN_FORWARD_SLOP;
if (forwardSlop < 0.0f) {
// BIG step, no slop necessary
forwardSlop = 0.0f;
}
// we push the step forward by stepMargin to help reduce accidental penetration
const btScalar MIN_STEP_MARGIN = 0.04f;
btScalar stepMargin = glm::max(_radius, MIN_STEP_MARGIN);
btScalar expandedStepLength = stepLength + forwardSlop + stepMargin;
// loop over topPoints
bool walkable = true;
for (int32_t i = 0; i < _topPoints.size(); ++i) {
rayStart = newPosition + rotation * _topPoints[i];
rayEnd = rayStart + expandedStepLength * stepDirection;
// reset rayResult for next test
rayResult.m_closestHitFraction = 1.0f;
rayResult.m_collisionObject = nullptr;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
if (rayResult.m_hitNormalWorld.dot(_currentUp) < _minFloorNormalDotUp) {
walkable = false;
break;
}
}
}
// scan the bottom
// TODO: implement sliding along sloped floors
bool steppingUp = false;
expandedStepLength = stepLength + MIN_FORWARD_SLOP + MIN_STEP_MARGIN;
for (int32_t i = _bottomPoints.size() - 1; i > -1; --i) {
rayStart = newPosition + rotation * _bottomPoints[i] - MIN_STEP_MARGIN * stepDirection;
rayEnd = rayStart + expandedStepLength * stepDirection;
// reset rayResult for next test
rayResult.m_closestHitFraction = 1.0f;
rayResult.m_collisionObject = nullptr;
if (_ghost.rayTest(rayStart, rayEnd, rayResult)) {
btScalar adjustedHitFraction = (rayResult.m_closestHitFraction * expandedStepLength - MIN_STEP_MARGIN) / (stepLength + MIN_FORWARD_SLOP);
if (adjustedHitFraction < 1.0f) {
steppingUp = true;
break;
}
}
}
if (!walkable && steppingUp ) {
return glm::vec3(0.0f);
}
// else it might not be walkable, but we aren't steppingUp yet which means we can still move forward
// TODO: slide up ramps and fall off edges (then we can remove the vertical follow of Avatar's RigidBody)
return step;
}
btConvexHullShape* MyCharacterController::computeShape() const {
// HACK: the avatar collides using convex hull with a collision margin equal to
// the old capsule radius. Two points define a capsule and additional points are
// spread out at chest level to produce a slight taper toward the feet. This
// makes the avatar more likely to collide with vertical walls at a higher point
// and thus less likely to produce a single-point collision manifold below the
// _maxStepHeight when walking into against vertical surfaces --> fixes a bug
// where the "walk up steps" feature would allow the avatar to walk up vertical
// walls.
const int32_t NUM_POINTS = 6;
btVector3 points[NUM_POINTS];
btVector3 xAxis = btVector3(1.0f, 0.0f, 0.0f);
btVector3 yAxis = btVector3(0.0f, 1.0f, 0.0f);
btVector3 zAxis = btVector3(0.0f, 0.0f, 1.0f);
points[0] = _halfHeight * yAxis;
points[1] = -_halfHeight * yAxis;
points[2] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * zAxis;
points[3] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * zAxis;
points[4] = (0.75f * _halfHeight) * yAxis - (0.1f * _radius) * xAxis;
points[5] = (0.75f * _halfHeight) * yAxis + (0.1f * _radius) * xAxis;
btConvexHullShape* shape = new btConvexHullShape(reinterpret_cast<btScalar*>(points), NUM_POINTS);
shape->setMargin(_radius);
return shape;
}
void MyCharacterController::initRayShotgun(const btCollisionWorld* world) {
// In order to trace rays out from the avatar's shape surface we need to know where the start points are in
// the local-frame. Since the avatar shape is somewhat irregular computing these points by hand is a hassle
// so instead we ray-trace backwards to the avatar to find them.
//
// We trace back a regular grid (see below) of points against the shape and keep any that hit.
// ___
// + / + \ +
// |+ +|
// +| + | +
// |+ +|
// +| + | +
// |+ +|
// + \ + / +
// ---
// The shotgun will send rays out from these same points to see if the avatar's shape can proceed through space.
// helper class for simple ray-traces against character
class MeOnlyResultCallback : public btCollisionWorld::ClosestRayResultCallback {
public:
MeOnlyResultCallback (btRigidBody* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)) {
_me = me;
m_collisionFilterGroup = BULLET_COLLISION_GROUP_DYNAMIC;
m_collisionFilterMask = BULLET_COLLISION_MASK_DYNAMIC;
}
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) override {
if (rayResult.m_collisionObject != _me) {
return 1.0f;
}
return ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);
}
btRigidBody* _me;
};
const btScalar fullHalfHeight = _radius + _halfHeight;
const btScalar divisionLine = -fullHalfHeight + _maxStepHeight; // line between top and bottom
const btScalar topHeight = fullHalfHeight - divisionLine;
const btScalar slop = 0.02f;
const int32_t NUM_ROWS = 5; // must be odd number > 1
const int32_t NUM_COLUMNS = 5; // must be odd number > 1
btVector3 reach = (2.0f * _radius) * btVector3(0.0f, 0.0f, 1.0f);
{ // top points
_topPoints.clear();
_topPoints.reserve(NUM_ROWS * NUM_COLUMNS);
btScalar stepY = (topHeight - slop) / (btScalar)(NUM_ROWS - 1);
btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(NUM_COLUMNS - 1);
btTransform transform = _rigidBody->getWorldTransform();
btVector3 position = transform.getOrigin();
btMatrix3x3 rotation = transform.getBasis();
for (int32_t i = 0; i < NUM_ROWS; ++i) {
int32_t maxJ = NUM_COLUMNS;
btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX;
if (i % 2 == 1) {
// odd rows have one less point and start a halfStep closer
maxJ -= 1;
offsetX += 0.5f * stepX;
}
for (int32_t j = 0; j < maxJ; ++j) {
btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, divisionLine + (btScalar)(i) * stepY, 0.0f);
btVector3 localRayStart = localRayEnd - reach;
MeOnlyResultCallback result(_rigidBody);
world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result);
if (result.m_closestHitFraction < 1.0f) {
_topPoints.push_back(localRayStart + result.m_closestHitFraction * reach);
}
}
}
}
{ // bottom points
_bottomPoints.clear();
_bottomPoints.reserve(NUM_ROWS * NUM_COLUMNS);
btScalar steepestStepHitHeight = (_radius + 0.04f) * (1.0f - DEFAULT_MIN_FLOOR_NORMAL_DOT_UP);
btScalar stepY = (_maxStepHeight - slop - steepestStepHitHeight) / (btScalar)(NUM_ROWS - 1);
btScalar stepX = 2.0f * (_radius - slop) / (btScalar)(NUM_COLUMNS - 1);
btTransform transform = _rigidBody->getWorldTransform();
btVector3 position = transform.getOrigin();
btMatrix3x3 rotation = transform.getBasis();
for (int32_t i = 0; i < NUM_ROWS; ++i) {
int32_t maxJ = NUM_COLUMNS;
btScalar offsetX = -(btScalar)((NUM_COLUMNS - 1) / 2) * stepX;
if (i % 2 == 1) {
// odd rows have one less point and start a halfStep closer
maxJ -= 1;
offsetX += 0.5f * stepX;
}
for (int32_t j = 0; j < maxJ; ++j) {
btVector3 localRayEnd(offsetX + (btScalar)(j) * stepX, (divisionLine - slop) - (btScalar)(i) * stepY, 0.0f);
btVector3 localRayStart = localRayEnd - reach;
MeOnlyResultCallback result(_rigidBody);
world->rayTest(position + rotation * localRayStart, position + rotation * localRayEnd, result);
if (result.m_closestHitFraction < 1.0f) {
_bottomPoints.push_back(localRayStart + result.m_closestHitFraction * reach);
}
}
}
}
}

29
interface/src/avatar/MyCharacterController.h
View file

@ -24,10 +24,37 @@ public:
explicit MyCharacterController(MyAvatar* avatar);
~MyCharacterController ();
virtual void updateShapeIfNecessary() override;
void setDynamicsWorld(btDynamicsWorld* world) override;
void updateShapeIfNecessary() override;
// Sweeping a convex shape through the physics simulation can expensive when the obstacles are too complex
// (e.g. small 20k triangle static mesh) so instead as a fallback we cast several rays forward and if they
// don't hit anything we consider it a clean sweep. Hence the "Shotgun" code.
class RayShotgunResult {
public:
void reset();
float hitFraction { 1.0f };
bool walkable { true };
};
/// return true if RayShotgun hits anything
bool testRayShotgun(const glm::vec3& position, const glm::vec3& step, RayShotgunResult& result);
glm::vec3 computeHMDStep(const glm::vec3& position, const glm::vec3& step);
protected:
void initRayShotgun(const btCollisionWorld* world);
private:
btConvexHullShape* computeShape() const;
protected:
MyAvatar* _avatar { nullptr };
// shotgun scan data
btAlignedObjectArray<btVector3> _topPoints;
btAlignedObjectArray<btVector3> _bottomPoints;
};
#endif // hifi_MyCharacterController_h

67
interface/src/avatar/SkeletonModel.cpp
View file

@ -23,6 +23,12 @@
#include "InterfaceLogging.h"
#include "AnimDebugDraw.h"
glm::vec3 TRUNCATE_IK_CAPSULE_POSITION(0.0f, 0.0f, 0.0f);
float TRUNCATE_IK_CAPSULE_LENGTH = 1000.0f;
float TRUNCATE_IK_CAPSULE_RADIUS = 0.25f;
float MIN_OUT_OF_BODY_DISTANCE = TRUNCATE_IK_CAPSULE_RADIUS - 0.1f;
float MAX_OUT_OF_BODY_DISTANCE = TRUNCATE_IK_CAPSULE_RADIUS + 0.1f;
SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent, RigPointer rig) :
Model(rig, parent),
_owningAvatar(owningAvatar),
@ -86,7 +92,6 @@ Rig::CharacterControllerState convertCharacterControllerState(CharacterControlle
};
}
// Called within Model::simulate call, below.
void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
const FBXGeometry& geometry = getFBXGeometry();
@ -107,6 +112,9 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
Rig::HeadParameters headParams;
glm::vec3 hmdPositionInRigSpace;
glm::vec3 truncatedHMDPositionInRigSpace;
if (qApp->isHMDMode()) {
headParams.isInHMD = true;
@ -116,9 +124,22 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
glm::mat4 worldToRig = glm::inverse(rigToWorld);
glm::mat4 rigHMDMat = worldToRig * worldHMDMat;
headParams.rigHeadPosition = extractTranslation(rigHMDMat);
hmdPositionInRigSpace = extractTranslation(rigHMDMat);
glm::vec3 capsuleStart = Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
glm::vec3 capsuleEnd = -Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
// truncate head IK target if it's out of body
if (myAvatar->isOutOfBody()) {
truncatedHMDPositionInRigSpace = projectPointOntoCapsule(hmdPositionInRigSpace, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS);
} else {
truncatedHMDPositionInRigSpace = hmdPositionInRigSpace;
}
headParams.rigHeadPosition = truncatedHMDPositionInRigSpace;
headParams.rigHeadOrientation = extractRotation(rigHMDMat);
headParams.worldHeadOrientation = extractRotation(worldHMDMat);
} else {
headParams.isInHMD = false;
@ -132,13 +153,42 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
_rig->updateFromHeadParameters(headParams, deltaTime);
// OUTOFBODY_HACK: clamp horizontal component of head by maxHipsOffset.
// This is to prevent the hands from being incorrect relative to the head because
// the hips are being constrained by a small maxHipsOffset due to collision.
if (myAvatar->isOutOfBody()) {
float headOffsetLength2D = glm::length(glm::vec2(truncatedHMDPositionInRigSpace.x, truncatedHMDPositionInRigSpace.z));
if (headOffsetLength2D > _rig->getMaxHipsOffsetLength()) {
truncatedHMDPositionInRigSpace.x *= _rig->getMaxHipsOffsetLength() / headOffsetLength2D;
truncatedHMDPositionInRigSpace.z *= _rig->getMaxHipsOffsetLength() / headOffsetLength2D;
}
}
Rig::HandParameters handParams;
// compute interp factor between in body and out of body hand positions.
glm::vec3 capsuleStart = Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
glm::vec3 capsuleEnd = -Vectors::UNIT_Y * (TRUNCATE_IK_CAPSULE_LENGTH / 2.0f);
float outOfBodyAlpha = distanceFromCapsule(hmdPositionInRigSpace, capsuleStart, capsuleEnd, TRUNCATE_IK_CAPSULE_RADIUS);
outOfBodyAlpha = (glm::clamp(outOfBodyAlpha, MIN_OUT_OF_BODY_DISTANCE, MAX_OUT_OF_BODY_DISTANCE) - MIN_OUT_OF_BODY_DISTANCE) /
(MAX_OUT_OF_BODY_DISTANCE - MIN_OUT_OF_BODY_DISTANCE);
auto leftPose = myAvatar->getLeftHandControllerPoseInAvatarFrame();
if (leftPose.isValid()) {
handParams.isLeftEnabled = true;
handParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation();
handParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation();
// adjust hand position if head is out of body.
if (qApp->isHMDMode()) {
// compute the out of body hand position.
glm::vec3 offset = handParams.leftPosition - hmdPositionInRigSpace;
glm::vec3 outOfBodyLeftPosition = truncatedHMDPositionInRigSpace + offset;
// interpolate between in body and out of body hand position.
handParams.leftPosition = lerp(handParams.leftPosition, outOfBodyLeftPosition, outOfBodyAlpha);
}
} else {
handParams.isLeftEnabled = false;
}
@ -148,6 +198,17 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
handParams.isRightEnabled = true;
handParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation();
handParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation();
// adjust hand position if head is out of body.
if (qApp->isHMDMode()) {
// compute the out of body hand position.
glm::vec3 offset = handParams.rightPosition - hmdPositionInRigSpace;
glm::vec3 outOfBodyRightPosition = truncatedHMDPositionInRigSpace + offset;
// interpolate between in body and out of body hand position.
handParams.rightPosition = lerp(handParams.rightPosition, outOfBodyRightPosition, outOfBodyAlpha);
}
} else {
handParams.isRightEnabled = false;
}
@ -160,7 +221,7 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState());
auto velocity = myAvatar->getLocalVelocity();
auto velocity = myAvatar->getPreActionVelocity();
auto position = myAvatar->getLocalPosition();
auto orientation = myAvatar->getLocalOrientation();
_rig->computeMotionAnimationState(deltaTime, position, velocity, orientation, ccState);

20
interface/src/scripting/HMDScriptingInterface.cpp
View file

@ -114,6 +114,21 @@ glm::vec3 HMDScriptingInterface::getPosition() const {
return glm::vec3();
}
void HMDScriptingInterface::setPosition(const glm::vec3& position) {
if (QThread::currentThread() != thread()) {
QMetaObject::invokeMethod(this, "setPosition", Qt::QueuedConnection, Q_ARG(const glm::vec3&, position));
return;
} else {
auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
glm::mat4 hmdToSensor = myAvatar->getHMDSensorMatrix();
glm::mat4 sensorToWorld = myAvatar->getSensorToWorldMatrix();
glm::mat4 hmdToWorld = sensorToWorld * hmdToSensor;
setTranslation(hmdToWorld, position);
sensorToWorld = hmdToWorld * glm::inverse(hmdToSensor);
myAvatar->setSensorToWorldMatrix(sensorToWorld);
}
}
glm::quat HMDScriptingInterface::getOrientation() const {
if (qApp->getActiveDisplayPlugin()->isHmd()) {
return glm::normalize(glm::quat_cast(getWorldHMDMatrix()));
@ -184,3 +199,8 @@ bool HMDScriptingInterface::isKeyboardVisible() {
void HMDScriptingInterface::centerUI() {
QMetaObject::invokeMethod(qApp, "centerUI", Qt::QueuedConnection);
}
void HMDScriptingInterface::snapToAvatar() {
auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
myAvatar->updateSensorToWorldMatrix();
}

12
interface/src/scripting/HMDScriptingInterface.h
View file

@ -25,7 +25,7 @@ class QScriptEngine;
class HMDScriptingInterface : public AbstractHMDScriptingInterface, public Dependency {
Q_OBJECT
Q_PROPERTY(glm::vec3 position READ getPosition)
Q_PROPERTY(glm::vec3 position READ getPosition WRITE setPosition)
Q_PROPERTY(glm::quat orientation READ getOrientation)
Q_PROPERTY(bool mounted READ isMounted)
@ -56,7 +56,7 @@ public:
/// not be interrupted by a keyboard popup
/// Returns false if there is already an active keyboard displayed.
/// Clients should re-enable the keyboard when the operation is complete and ensure
/// that they balance any call to suppressKeyboard() that returns true with a corresponding
/// that they balance any call to suppressKeyboard() that returns true with a corresponding
/// call to unsuppressKeyboard() within a reasonable amount of time
Q_INVOKABLE bool suppressKeyboard();
@ -69,6 +69,9 @@ public:
// rotate the overlay UI sphere so that it is centered about the the current HMD position and orientation
Q_INVOKABLE void centerUI();
// snap HMD to align with Avatar's current position in world-frame
Q_INVOKABLE void snapToAvatar();
signals:
bool shouldShowHandControllersChanged();
@ -82,7 +85,10 @@ public:
private:
// Get the position of the HMD
glm::vec3 getPosition() const;
// Set the position of the HMD
Q_INVOKABLE void setPosition(const glm::vec3& position);
// Get the orientation of the HMD
glm::quat getOrientation() const;

14
libraries/animation/src/AnimBlendLinear.cpp
View file

@ -24,7 +24,7 @@ AnimBlendLinear::~AnimBlendLinear() {
}
const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) {
const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) {
_alpha = animVars.lookup(_alphaVar, _alpha);
@ -33,7 +33,7 @@ const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, flo
pose = AnimPose::identity;
}
} else if (_children.size() == 1) {
_poses = _children[0]->evaluate(animVars, dt, triggersOut);
_poses = _children[0]->evaluate(animVars, context, dt, triggersOut);
} else {
float clampedAlpha = glm::clamp(_alpha, 0.0f, (float)(_children.size() - 1));
@ -41,7 +41,7 @@ const AnimPoseVec& AnimBlendLinear::evaluate(const AnimVariantMap& animVars, flo
size_t nextPoseIndex = glm::ceil(clampedAlpha);
float alpha = glm::fract(clampedAlpha);
evaluateAndBlendChildren(animVars, triggersOut, alpha, prevPoseIndex, nextPoseIndex, dt);
evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, dt);
}
return _poses;
}
@ -51,15 +51,15 @@ const AnimPoseVec& AnimBlendLinear::getPosesInternal() const {
return _poses;
}
void AnimBlendLinear::evaluateAndBlendChildren(const AnimVariantMap& animVars, Triggers& triggersOut, float alpha,
void AnimBlendLinear::evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha,
size_t prevPoseIndex, size_t nextPoseIndex, float dt) {
if (prevPoseIndex == nextPoseIndex) {
// this can happen if alpha is on an integer boundary
_poses = _children[prevPoseIndex]->evaluate(animVars, dt, triggersOut);
_poses = _children[prevPoseIndex]->evaluate(animVars, context, dt, triggersOut);
} else {
// need to eval and blend between two children.
auto prevPoses = _children[prevPoseIndex]->evaluate(animVars, dt, triggersOut);
auto nextPoses = _children[nextPoseIndex]->evaluate(animVars, dt, triggersOut);
auto prevPoses = _children[prevPoseIndex]->evaluate(animVars, context, dt, triggersOut);
auto nextPoses = _children[nextPoseIndex]->evaluate(animVars, context, dt, triggersOut);
if (prevPoses.size() > 0 && prevPoses.size() == nextPoses.size()) {
_poses.resize(prevPoses.size());

4
libraries/animation/src/AnimBlendLinear.h
View file

@ -30,7 +30,7 @@ public:
AnimBlendLinear(const QString& id, float alpha);
virtual ~AnimBlendLinear() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override;
void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; }
@ -38,7 +38,7 @@ protected:
// for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override;
void evaluateAndBlendChildren(const AnimVariantMap& animVars, Triggers& triggersOut, float alpha,
void evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha,
size_t prevPoseIndex, size_t nextPoseIndex, float dt);
AnimPoseVec _poses;

14
libraries/animation/src/AnimBlendLinearMove.cpp
View file

@ -26,7 +26,7 @@ AnimBlendLinearMove::~AnimBlendLinearMove() {
}
const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) {
const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) {
assert(_children.size() == _characteristicSpeeds.size());
@ -43,7 +43,7 @@ const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars,
const int nextPoseIndex = 0;
float prevDeltaTime, nextDeltaTime;
setFrameAndPhase(dt, alpha, prevPoseIndex, nextPoseIndex, &prevDeltaTime, &nextDeltaTime, triggersOut);
evaluateAndBlendChildren(animVars, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime);
evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime);
} else {
auto clampedAlpha = glm::clamp(_alpha, 0.0f, (float)(_children.size() - 1));
@ -52,7 +52,7 @@ const AnimPoseVec& AnimBlendLinearMove::evaluate(const AnimVariantMap& animVars,
auto alpha = glm::fract(clampedAlpha);
float prevDeltaTime, nextDeltaTime;
setFrameAndPhase(dt, alpha, prevPoseIndex, nextPoseIndex, &prevDeltaTime, &nextDeltaTime, triggersOut);
evaluateAndBlendChildren(animVars, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime);
evaluateAndBlendChildren(animVars, context, triggersOut, alpha, prevPoseIndex, nextPoseIndex, prevDeltaTime, nextDeltaTime);
}
return _poses;
}
@ -62,16 +62,16 @@ const AnimPoseVec& AnimBlendLinearMove::getPosesInternal() const {
return _poses;
}
void AnimBlendLinearMove::evaluateAndBlendChildren(const AnimVariantMap& animVars, Triggers& triggersOut, float alpha,
void AnimBlendLinearMove::evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha,
size_t prevPoseIndex, size_t nextPoseIndex,
float prevDeltaTime, float nextDeltaTime) {
if (prevPoseIndex == nextPoseIndex) {
// this can happen if alpha is on an integer boundary
_poses = _children[prevPoseIndex]->evaluate(animVars, prevDeltaTime, triggersOut);
_poses = _children[prevPoseIndex]->evaluate(animVars, context, prevDeltaTime, triggersOut);
} else {
// need to eval and blend between two children.
auto prevPoses = _children[prevPoseIndex]->evaluate(animVars, prevDeltaTime, triggersOut);
auto nextPoses = _children[nextPoseIndex]->evaluate(animVars, nextDeltaTime, triggersOut);
auto prevPoses = _children[prevPoseIndex]->evaluate(animVars, context, prevDeltaTime, triggersOut);
auto nextPoses = _children[nextPoseIndex]->evaluate(animVars, context, nextDeltaTime, triggersOut);
if (prevPoses.size() > 0 && prevPoses.size() == nextPoses.size()) {
_poses.resize(prevPoses.size());

4
libraries/animation/src/AnimBlendLinearMove.h
View file

@ -39,7 +39,7 @@ public:
AnimBlendLinearMove(const QString& id, float alpha, float desiredSpeed, const std::vector<float>& characteristicSpeeds);
virtual ~AnimBlendLinearMove() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override;
void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; }
void setDesiredSpeedVar(const QString& desiredSpeedVar) { _desiredSpeedVar = desiredSpeedVar; }
@ -48,7 +48,7 @@ protected:
// for AnimDebugDraw rendering
virtual const AnimPoseVec& getPosesInternal() const override;
void evaluateAndBlendChildren(const AnimVariantMap& animVars, Triggers& triggersOut, float alpha,
void evaluateAndBlendChildren(const AnimVariantMap& animVars, const AnimContext& context, Triggers& triggersOut, float alpha,
size_t prevPoseIndex, size_t nextPoseIndex,
float prevDeltaTime, float nextDeltaTime);

2
libraries/animation/src/AnimClip.cpp
View file

@ -31,7 +31,7 @@ AnimClip::~AnimClip() {
}
const AnimPoseVec& AnimClip::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) {
const AnimPoseVec& AnimClip::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) {
// lookup parameters from animVars, using current instance variables as defaults.
_startFrame = animVars.lookup(_startFrameVar, _startFrame);

2
libraries/animation/src/AnimClip.h
View file

@ -30,7 +30,7 @@ public:
AnimClip(const QString& id, const QString& url, float startFrame, float endFrame, float timeScale, bool loopFlag, bool mirrorFlag);
virtual ~AnimClip() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override;
void setStartFrameVar(const QString& startFrameVar) { _startFrameVar = startFrameVar; }
void setEndFrameVar(const QString& endFrameVar) { _endFrameVar = endFrameVar; }

16
libraries/animation/src/AnimContext.cpp Normal file
View file

@ -0,0 +1,16 @@
//
// AnimContext.cpp
//
// Created by Anthony J. Thibault on 9/19/16.
// Copyright (c) 2016 High Fidelity, Inc. All rights reserved.
//
// Distributed under the Apache License, Version 2.0.
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include "AnimContext.h"
AnimContext::AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix) :
_enableDebugDrawIKTargets(enableDebugDrawIKTargets),
_geometryToRigMatrix(geometryToRigMatrix) {
}

30
libraries/animation/src/AnimContext.h Normal file
View file

@ -0,0 +1,30 @@
//
// AnimContext.h
//
// Created by Anthony J. Thibault on 9/19/16.
// Copyright (c) 2016 High Fidelity, Inc. All rights reserved.
//
// 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_AnimContext_h
#define hifi_AnimContext_h
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
class AnimContext {
public:
AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix);
bool getEnableDebugDrawIKTargets() const { return _enableDebugDrawIKTargets; }
const glm::mat4& getGeometryToRigMatrix() const { return _geometryToRigMatrix; }
protected:
bool _enableDebugDrawIKTargets { false };
glm::mat4 _geometryToRigMatrix;
};
#endif // hifi_AnimContext_h

79
libraries/animation/src/AnimInverseKinematics.cpp
View file

@ -14,6 +14,8 @@
#include <NumericalConstants.h>
#include <SharedUtil.h>
#include <shared/NsightHelpers.h>
#include <DebugDraw.h>
#include "Rig.h"
#include "ElbowConstraint.h"
#include "SwingTwistConstraint.h"
@ -376,14 +378,14 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
}
//virtual
const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) {
const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) {
// don't call this function, call overlay() instead
assert(false);
return _relativePoses;
}
//virtual
const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
const float MAX_OVERLAY_DT = 1.0f / 30.0f; // what to clamp delta-time to in AnimInverseKinematics::overlay
if (dt > MAX_OVERLAY_DT) {
@ -437,6 +439,28 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
computeTargets(animVars, targets, underPoses);
}
// debug render ik targets
if (context.getEnableDebugDrawIKTargets()) {
const vec4 WHITE(1.0f);
glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3());
for (auto& target : targets) {
glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation());
glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat;
std::string name = "ikTarget" + std::to_string(target.getIndex());
DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE);
}
} else if (context.getEnableDebugDrawIKTargets() != _previousEnableDebugIKTargets) {
// remove markers if they were added last frame.
for (auto& target : targets) {
std::string name = "ikTarget" + std::to_string(target.getIndex());
DebugDraw::getInstance().removeMyAvatarMarker(name);
}
}
_previousEnableDebugIKTargets = context.getEnableDebugDrawIKTargets();
if (targets.empty()) {
// no IK targets but still need to enforce constraints
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
@ -486,7 +510,13 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
// measure new _hipsOffset for next frame
// by looking for discrepancies between where a targeted endEffector is
// and where it wants to be (after IK solutions are done)
glm::vec3 newHipsOffset = Vectors::ZERO;
// OUTOFBODY_HACK:use weighted average between HMD and other targets
float HMD_WEIGHT = 10.0f;
float OTHER_WEIGHT = 1.0f;
float totalWeight = 0.0f;
glm::vec3 additionalHipsOffset = Vectors::ZERO;
for (auto& target: targets) {
int targetIndex = target.getIndex();
if (targetIndex == _headIndex && _headIndex != -1) {
@ -497,26 +527,45 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans;
glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under);
additionalHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (under- actual);
totalWeight += OTHER_WEIGHT;
} else if (target.getType() == IKTarget::Type::HmdHead) {
// we want to shift the hips to bring the head to its designated position
glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
_hipsOffset += target.getTranslation() - actual;
// and ignore all other targets
newHipsOffset = _hipsOffset;
break;
glm::vec3 thisOffset = target.getTranslation() - actual;
glm::vec3 futureHipsOffset = _hipsOffset + thisOffset;
if (glm::length(glm::vec2(futureHipsOffset.x, futureHipsOffset.z)) < _maxHipsOffsetLength) {
// it is imperative to shift the hips and bring the head to its designated position
// so we slam newHipsOffset here and ignore all other targets
additionalHipsOffset = futureHipsOffset - _hipsOffset;
totalWeight = 0.0f;
break;
} else {
additionalHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual);
totalWeight += HMD_WEIGHT;
}
}
} else if (target.getType() == IKTarget::Type::RotationAndPosition) {
glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans;
glm::vec3 targetPosition = target.getTranslation();
newHipsOffset += targetPosition - actualPosition;
additionalHipsOffset += OTHER_WEIGHT * (targetPosition - actualPosition);
totalWeight += OTHER_WEIGHT;
}
}
if (totalWeight > 1.0f) {
additionalHipsOffset /= totalWeight;
}
// smooth transitions by relaxing _hipsOffset toward the new value
const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f;
const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f;
float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f;
_hipsOffset += (newHipsOffset - _hipsOffset) * tau;
_hipsOffset += additionalHipsOffset * tau;
// clamp the horizontal component of the hips offset
float hipsOffsetLength2D = glm::length(glm::vec2(_hipsOffset.x, _hipsOffset.z));
if (hipsOffsetLength2D > _maxHipsOffsetLength) {
_hipsOffset.x *= _maxHipsOffsetLength / hipsOffsetLength2D;
_hipsOffset.z *= _maxHipsOffsetLength / hipsOffsetLength2D;
}
}
}
}
@ -529,6 +578,12 @@ void AnimInverseKinematics::clearIKJointLimitHistory() {
}
}
void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) {
// OUTOFBODY_HACK: manually adjust scale here
const float METERS_TO_CENTIMETERS = 100.0f;
_maxHipsOffsetLength = METERS_TO_CENTIMETERS * maxLength;
}
RotationConstraint* AnimInverseKinematics::getConstraint(int index) {
RotationConstraint* constraint = nullptr;
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.find(index);

9
libraries/animation/src/AnimInverseKinematics.h
View file

@ -34,11 +34,13 @@ public:
void setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, const QString& typeVar);
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) override;
virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) override;
virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override;
void clearIKJointLimitHistory();
void setMaxHipsOffsetLength(float maxLength);
protected:
void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);
@ -83,6 +85,7 @@ protected:
// experimental data for moving hips during IK
glm::vec3 _hipsOffset { Vectors::ZERO };
float _maxHipsOffsetLength { 1.0f };
int _headIndex { -1 };
int _hipsIndex { -1 };
int _hipsParentIndex { -1 };
@ -90,6 +93,8 @@ protected:
// _maxTargetIndex is tracked to help optimize the recalculation of absolute poses
// during the the cyclic coordinate descent algorithm
int _maxTargetIndex { 0 };
bool _previousEnableDebugIKTargets { false };
};
#endif // hifi_AnimInverseKinematics_h

6
libraries/animation/src/AnimManipulator.cpp
View file

@ -22,11 +22,11 @@ AnimManipulator::~AnimManipulator() {
}
const AnimPoseVec& AnimManipulator::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) {
return overlay(animVars, dt, triggersOut, _skeleton->getRelativeBindPoses());
const AnimPoseVec& AnimManipulator::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) {
return overlay(animVars, context, dt, triggersOut, _skeleton->getRelativeBindPoses());
}
const AnimPoseVec& AnimManipulator::overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
const AnimPoseVec& AnimManipulator::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
_alpha = animVars.lookup(_alphaVar, _alpha);
_poses = underPoses;

4
libraries/animation/src/AnimManipulator.h
View file

@ -22,8 +22,8 @@ public:
AnimManipulator(const QString& id, float alpha);
virtual ~AnimManipulator() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override;
virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override;
virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override;
void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; }

8
libraries/animation/src/AnimNode.h
View file

@ -20,6 +20,7 @@
#include "AnimSkeleton.h"
#include "AnimVariant.h"
#include "AnimContext.h"
class QJsonObject;
@ -72,9 +73,10 @@ public:
AnimSkeleton::ConstPointer getSkeleton() const { return _skeleton; }
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) = 0;
virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
return evaluate(animVars, dt, triggersOut);
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) = 0;
virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut,
const AnimPoseVec& underPoses) {
return evaluate(animVars, context, dt, triggersOut);
}
void setCurrentFrame(float frame);

6
libraries/animation/src/AnimOverlay.cpp
View file

@ -39,7 +39,7 @@ void AnimOverlay::buildBoneSet(BoneSet boneSet) {
}
}
const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) {
const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) {
// lookup parameters from animVars, using current instance variables as defaults.
// NOTE: switching bonesets can be an expensive operation, let's try to avoid it.
@ -51,8 +51,8 @@ const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, float d
_alpha = animVars.lookup(_alphaVar, _alpha);
if (_children.size() >= 2) {
auto& underPoses = _children[1]->evaluate(animVars, dt, triggersOut);
auto& overPoses = _children[0]->overlay(animVars, dt, triggersOut, underPoses);
auto& underPoses = _children[1]->evaluate(animVars, context, dt, triggersOut);
auto& overPoses = _children[0]->overlay(animVars, context, dt, triggersOut, underPoses);
if (underPoses.size() > 0 && underPoses.size() == overPoses.size()) {
_poses.resize(underPoses.size());

2
libraries/animation/src/AnimOverlay.h
View file

@ -43,7 +43,7 @@ public:
AnimOverlay(const QString& id, BoneSet boneSet, float alpha);
virtual ~AnimOverlay() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override;
void setBoneSetVar(const QString& boneSetVar) { _boneSetVar = boneSetVar; }
void setAlphaVar(const QString& alphaVar) { _alphaVar = alphaVar; }

14
libraries/animation/src/AnimStateMachine.cpp
View file

@ -21,7 +21,7 @@ AnimStateMachine::~AnimStateMachine() {
}
const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) {
const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) {
QString desiredStateID = animVars.lookup(_currentStateVar, _currentState->getID());
if (_currentState->getID() != desiredStateID) {
@ -29,7 +29,7 @@ const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, fl
bool foundState = false;
for (auto& state : _states) {
if (state->getID() == desiredStateID) {
switchState(animVars, state);
switchState(animVars, context, state);
foundState = true;
break;
}
@ -42,7 +42,7 @@ const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, fl
// evaluate currentState transitions
auto desiredState = evaluateTransitions(animVars);
if (desiredState != _currentState) {
switchState(animVars, desiredState);
switchState(animVars, context, desiredState);
}
assert(_currentState);
@ -62,7 +62,7 @@ const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, fl
} else if (_interpType == InterpType::SnapshotPrev) {
// interp between the prev snapshot and evaluated next target.
// this is useful for interping into a blend
localNextPoses = currentStateNode->evaluate(animVars, dt, triggersOut);
localNextPoses = currentStateNode->evaluate(animVars, context, dt, triggersOut);
prevPoses = &_prevPoses;
nextPoses = &localNextPoses;
} else {
@ -79,7 +79,7 @@ const AnimPoseVec& AnimStateMachine::evaluate(const AnimVariantMap& animVars, fl
}
}
if (!_duringInterp) {
_poses = currentStateNode->evaluate(animVars, dt, triggersOut);
_poses = currentStateNode->evaluate(animVars, context, dt, triggersOut);
}
return _poses;
}
@ -92,7 +92,7 @@ void AnimStateMachine::addState(State::Pointer state) {
_states.push_back(state);
}
void AnimStateMachine::switchState(const AnimVariantMap& animVars, State::Pointer desiredState) {
void AnimStateMachine::switchState(const AnimVariantMap& animVars, const AnimContext& context, State::Pointer desiredState) {
const float FRAMES_PER_SECOND = 30.0f;
@ -114,7 +114,7 @@ void AnimStateMachine::switchState(const AnimVariantMap& animVars, State::Pointe
_prevPoses = _poses;
// snapshot next pose at the target frame.
nextStateNode->setCurrentFrame(desiredState->_interpTarget);
_nextPoses = nextStateNode->evaluate(animVars, dt, triggers);
_nextPoses = nextStateNode->evaluate(animVars, context, dt, triggers);
} else if (_interpType == InterpType::SnapshotPrev) {
// snapshot previoius pose
_prevPoses = _poses;

4
libraries/animation/src/AnimStateMachine.h
View file

@ -113,7 +113,7 @@ public:
explicit AnimStateMachine(const QString& id);
virtual ~AnimStateMachine() override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, float dt, Triggers& triggersOut) override;
virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut) override;
void setCurrentStateVar(QString& currentStateVar) { _currentStateVar = currentStateVar; }
@ -123,7 +123,7 @@ protected:
void addState(State::Pointer state);
void switchState(const AnimVariantMap& animVars, State::Pointer desiredState);
void switchState(const AnimVariantMap& animVars, const AnimContext& context, State::Pointer desiredState);
State::Pointer evaluateTransitions(const AnimVariantMap& animVars) const;
// for AnimDebugDraw rendering

87
libraries/animation/src/Rig.cpp
View file

@ -298,7 +298,6 @@ void Rig::clearJointAnimationPriority(int index) {
void Rig::clearIKJointLimitHistory() {
if (_animNode) {
_animNode->traverse([&](AnimNode::Pointer node) {
// only report clip nodes as valid roles.
auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
if (ikNode) {
ikNode->clearIKJointLimitHistory();
@ -308,6 +307,30 @@ void Rig::clearIKJointLimitHistory() {
}
}
void Rig::updateMaxHipsOffsetLength(float maxLength, float deltaTime) {
_desiredMaxHipsOffsetLength = maxLength;
// OUTOFBODY_HACK: smoothly update update _hipsOffsetLength, otherwise we risk introducing oscillation in the hips offset.
const float MAX_HIPS_OFFSET_TIMESCALE = 0.33f;
float tau = deltaTime / MAX_HIPS_OFFSET_TIMESCALE;
_maxHipsOffsetLength = (1.0f - tau) * _maxHipsOffsetLength + tau * _desiredMaxHipsOffsetLength;
if (_animNode) {
_animNode->traverse([&](AnimNode::Pointer node) {
auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
if (ikNode) {
ikNode->setMaxHipsOffsetLength(_maxHipsOffsetLength);
}
return true;
});
}
}
float Rig::getMaxHipsOffsetLength() const {
return _maxHipsOffsetLength;
}
int Rig::getJointParentIndex(int childIndex) const {
if (_animSkeleton && isIndexValid(childIndex)) {
return _animSkeleton->getParentIndex(childIndex);
@ -495,15 +518,22 @@ bool Rig::getRelativeDefaultJointTranslation(int index, glm::vec3& translationOu
}
// animation reference speeds.
static const std::vector<float> FORWARD_SPEEDS = { 0.4f, 1.4f, 4.5f }; // m/s
static const std::vector<float> BACKWARD_SPEEDS = { 0.6f, 1.45f }; // m/s
static const std::vector<float> LATERAL_SPEEDS = { 0.2f, 0.65f }; // m/s
static const std::vector<float> FORWARD_SPEEDS = { 0.4f, 1.3f, 4.5f }; // m/s
static const std::vector<float> BACKWARD_SPEEDS = { 0.6f, 1.05f }; // m/s
static const std::vector<float> LATERAL_SPEEDS = { 0.2f, 0.5f }; // m/s
static const float DEFAULT_AVATAR_EYE_HEIGHT = 1.65f; // movement speeds are for characters of this eye-height. ~170 cm tall.
void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState) {
glm::vec3 front = worldRotation * IDENTITY_FRONT;
glm::vec3 workingVelocity = worldVelocity;
// TODO: account for avatar scaling
int eyeJoint = indexOfJoint("LeftEye");
int toeJoint = indexOfJoint("LeftToeBase");
const float AVATAR_EYE_HEIGHT = (eyeJoint >= 0 && toeJoint >= 0) ? getAbsoluteDefaultPose(eyeJoint).trans.y - getAbsoluteDefaultPose(toeJoint).trans.y : DEFAULT_AVATAR_EYE_HEIGHT;
const float AVATAR_HEIGHT_RATIO = DEFAULT_AVATAR_EYE_HEIGHT / AVATAR_EYE_HEIGHT;
{
glm::vec3 localVel = glm::inverse(worldRotation) * workingVelocity;
@ -523,18 +553,22 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos
float moveBackwardAlpha = 0.0f;
float moveLateralAlpha = 0.0f;
// calcuate the animation alpha and timeScale values based on current speeds and animation reference speeds.
calcAnimAlpha(_averageForwardSpeed.getAverage(), FORWARD_SPEEDS, &moveForwardAlpha);
calcAnimAlpha(-_averageForwardSpeed.getAverage(), BACKWARD_SPEEDS, &moveBackwardAlpha);
calcAnimAlpha(fabsf(_averageLateralSpeed.getAverage()), LATERAL_SPEEDS, &moveLateralAlpha);
float averageForwardSpeed = AVATAR_HEIGHT_RATIO * _averageForwardSpeed.getAverage();
float averageBackwardSpeed = -averageForwardSpeed;
float averageLateralSpeed = AVATAR_HEIGHT_RATIO * fabsf(_averageLateralSpeed.getAverage());
_animVars.set("moveForwardSpeed", _averageForwardSpeed.getAverage());
// calcuate the animation alpha and timeScale values based on current speeds and animation reference speeds.
calcAnimAlpha(averageForwardSpeed, FORWARD_SPEEDS, &moveForwardAlpha);
calcAnimAlpha(averageBackwardSpeed, BACKWARD_SPEEDS, &moveBackwardAlpha);
calcAnimAlpha(averageLateralSpeed, LATERAL_SPEEDS, &moveLateralAlpha);
_animVars.set("moveForwardSpeed", averageForwardSpeed);
_animVars.set("moveForwardAlpha", moveForwardAlpha);
_animVars.set("moveBackwardSpeed", -_averageForwardSpeed.getAverage());
_animVars.set("moveBackwardSpeed", averageBackwardSpeed);
_animVars.set("moveBackwardAlpha", moveBackwardAlpha);
_animVars.set("moveLateralSpeed", fabsf(_averageLateralSpeed.getAverage()));
_animVars.set("moveLateralSpeed", averageLateralSpeed);
_animVars.set("moveLateralAlpha", moveLateralAlpha);
const float MOVE_ENTER_SPEED_THRESHOLD = 0.2f; // m/sec
@ -592,7 +626,7 @@ void Rig::computeMotionAnimationState(float deltaTime, const glm::vec3& worldPos
}
}
const float STATE_CHANGE_HYSTERESIS_TIMER = 0.1f;
const float STATE_CHANGE_HYSTERESIS_TIMER = 1.0f / 60.0f;
// Skip hystersis timer for jump transitions.
if (_desiredState == RigRole::Takeoff) {
@ -891,9 +925,11 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
updateAnimationStateHandlers();
_animVars.setRigToGeometryTransform(_rigToGeometryTransform);
AnimContext context(_enableDebugDrawIKTargets, getGeometryToRigTransform());
// evaluate the animation
AnimNode::Triggers triggersOut;
_internalPoseSet._relativePoses = _animNode->evaluate(_animVars, deltaTime, triggersOut);
_internalPoseSet._relativePoses = _animNode->evaluate(_animVars, context, deltaTime, triggersOut);
if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) {
// animations haven't fully loaded yet.
_internalPoseSet._relativePoses = _animSkeleton->getRelativeDefaultPoses();
@ -1022,20 +1058,6 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) {
AnimPose hmdPose(glm::vec3(1.0f), params.rigHeadOrientation * yFlip180, params.rigHeadPosition);
computeHeadNeckAnimVars(hmdPose, headPos, headRot, neckPos, neckRot);
// debug rendering
#ifdef DEBUG_RENDERING
const glm::vec4 red(1.0f, 0.0f, 0.0f, 1.0f);
const glm::vec4 green(0.0f, 1.0f, 0.0f, 1.0f);
// transform from bone into avatar space
AnimPose headPose(glm::vec3(1), headRot, headPos);
DebugDraw::getInstance().addMyAvatarMarker("headTarget", headPose.rot, headPose.trans, red);
// transform from bone into avatar space
AnimPose neckPose(glm::vec3(1), neckRot, neckPos);
DebugDraw::getInstance().addMyAvatarMarker("neckTarget", neckPose.rot, neckPose.trans, green);
#endif
_animVars.set("headPosition", headPos);
_animVars.set("headRotation", headRot);
_animVars.set("headType", (int)IKTarget::Type::HmdHead);
@ -1108,8 +1130,9 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
if (params.isLeftEnabled) {
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 handPosition = params.leftPosition;
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 displacement;
if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
handPosition -= displacement;
@ -1126,8 +1149,9 @@ void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
if (params.isRightEnabled) {
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 handPosition = params.rightPosition;
// prevent the hand IK targets from intersecting the body capsule
glm::vec3 displacement;
if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
handPosition -= displacement;
@ -1379,9 +1403,10 @@ void Rig::computeAvatarBoundingCapsule(
// call overlay twice: once to verify AnimPoseVec joints and again to do the IK
AnimNode::Triggers triggersOut;
AnimContext context(false, glm::mat4());
float dt = 1.0f; // the value of this does not matter
ikNode.overlay(animVars, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
AnimPoseVec finalPoses = ikNode.overlay(animVars, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
AnimPoseVec finalPoses = ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
// convert relative poses to absolute
_animSkeleton->convertRelativePosesToAbsolute(finalPoses);

10
libraries/animation/src/Rig.h
View file

@ -104,6 +104,8 @@ public:
void clearJointAnimationPriority(int index);
void clearIKJointLimitHistory();
void updateMaxHipsOffsetLength(float maxLength, float deltaTime);
float getMaxHipsOffsetLength() const;
int getJointParentIndex(int childIndex) const;
@ -213,6 +215,8 @@ public:
const glm::mat4& getGeometryToRigTransform() const { return _geometryToRigTransform; }
void setEnableDebugDrawIKTargets(bool enableDebugDrawIKTargets) { _enableDebugDrawIKTargets = enableDebugDrawIKTargets; }
signals:
void onLoadComplete();
@ -315,6 +319,12 @@ protected:
bool _enableInverseKinematics { true };
mutable uint32_t _jointNameWarningCount { 0 };
glm::vec3 _desiredRigHeadPosition;
bool _truncateIKTargets { false };
bool _enableDebugDrawIKTargets { false };
float _maxHipsOffsetLength { 1.0f };
float _desiredMaxHipsOffsetLength { 1.0f };
private:
QMap<int, StateHandler> _stateHandlers;

2
libraries/physics/src/BulletUtil.h
View file

@ -1,6 +1,6 @@
//
// BulletUtil.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.11.02
// Copyright 2014 High Fidelity, Inc.

600
libraries/physics/src/CharacterController.cpp Normal file → Executable file
View file

@ -1,6 +1,6 @@
//
// CharacterControllerInterface.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2015.10.21
// Copyright 2015 High Fidelity, Inc.
@ -13,8 +13,8 @@
#include <NumericalConstants.h>
#include "PhysicsCollisionGroups.h"
#include "ObjectMotionState.h"
#include "PhysicsHelpers.h"
#include "PhysicsLogging.h"
const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f);
@ -62,15 +62,10 @@ CharacterController::CharacterMotor::CharacterMotor(const glm::vec3& vel, const
}
CharacterController::CharacterController() {
_halfHeight = 1.0f;
_enabled = false;
_floorDistance = MAX_FALL_HEIGHT;
_targetVelocity.setValue(0.0f, 0.0f, 0.0f);
_followDesiredBodyTransform.setIdentity();
_followTimeRemaining = 0.0f;
_jumpSpeed = JUMP_SPEED;
_state = State::Hover;
_isPushingUp = false;
@ -78,9 +73,6 @@ CharacterController::CharacterController() {
_takeoffToInAirStartTime = 0;
_jumpButtonDownStartTime = 0;
_jumpButtonDownCount = 0;
_followTime = 0.0f;
_followLinearDisplacement = btVector3(0, 0, 0);
_followAngularDisplacement = btQuaternion::getIdentity();
_hasSupport = false;
_pendingFlags = PENDING_FLAG_UPDATE_SHAPE;
@ -107,6 +99,7 @@ bool CharacterController::needsAddition() const {
void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
if (_dynamicsWorld != world) {
// remove from old world
if (_dynamicsWorld) {
if (_rigidBody) {
_dynamicsWorld->removeRigidBody(_rigidBody);
@ -115,16 +108,26 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
_dynamicsWorld = nullptr;
}
if (world && _rigidBody) {
// add to new world
_dynamicsWorld = world;
_pendingFlags &= ~PENDING_FLAG_JUMP;
// Before adding the RigidBody to the world we must save its oldGravity to the side
// because adding an object to the world will overwrite it with the default gravity.
btVector3 oldGravity = _rigidBody->getGravity();
_dynamicsWorld->addRigidBody(_rigidBody, BULLET_COLLISION_GROUP_MY_AVATAR, BULLET_COLLISION_MASK_MY_AVATAR);
_dynamicsWorld->addRigidBody(_rigidBody, _collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR);
_dynamicsWorld->addAction(this);
// restore gravity settings
_rigidBody->setGravity(oldGravity);
// restore gravity settings because adding an object to the world overwrites its gravity setting
_rigidBody->setGravity(_gravity * _currentUp);
btCollisionShape* shape = _rigidBody->getCollisionShape();
assert(shape && shape->getShapeType() == CONVEX_HULL_SHAPE_PROXYTYPE);
_ghost.setCharacterShape(static_cast<btConvexHullShape*>(shape));
}
// KINEMATIC_CONTROLLER_HACK
_ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup));
_ghost.setCollisionWorld(_dynamicsWorld);
_ghost.setRadiusAndHalfHeight(_radius, _halfHeight);
_ghost.setMaxStepHeight(0.75f * (_radius + _halfHeight)); // HACK
_ghost.setMinWallAngle(PI / 4.0f); // HACK
_ghost.setUpDirection(_currentUp);
_ghost.setMotorOnly(!_moveKinematically);
_ghost.setWorldTransform(_rigidBody->getWorldTransform());
}
if (_dynamicsWorld) {
if (_pendingFlags & PENDING_FLAG_UPDATE_SHAPE) {
@ -138,38 +141,84 @@ void CharacterController::setDynamicsWorld(btDynamicsWorld* world) {
}
}
static const float COS_PI_OVER_THREE = cosf(PI / 3.0f);
bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld, btScalar dt) {
if (_moveKinematically) {
// kinematic motion will move() the _ghost later
return _ghost.hasSupport();
}
bool pushing = _targetVelocity.length2() > FLT_EPSILON;
btDispatcher* dispatcher = collisionWorld->getDispatcher();
int numManifolds = dispatcher->getNumManifolds();
bool hasFloor = false;
btTransform rotation = _rigidBody->getWorldTransform();
rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f)); // clear translation part
bool CharacterController::checkForSupport(btCollisionWorld* collisionWorld) const {
int numManifolds = collisionWorld->getDispatcher()->getNumManifolds();
for (int i = 0; i < numManifolds; i++) {
btPersistentManifold* contactManifold = collisionWorld->getDispatcher()->getManifoldByIndexInternal(i);
const btCollisionObject* obA = static_cast<const btCollisionObject*>(contactManifold->getBody0());
const btCollisionObject* obB = static_cast<const btCollisionObject*>(contactManifold->getBody1());
if (obA == _rigidBody || obB == _rigidBody) {
btPersistentManifold* contactManifold = dispatcher->getManifoldByIndexInternal(i);
if (_rigidBody == contactManifold->getBody1() || _rigidBody == contactManifold->getBody0()) {
bool characterIsFirst = _rigidBody == contactManifold->getBody0();
int numContacts = contactManifold->getNumContacts();
int stepContactIndex = -1;
float highestStep = _minStepHeight;
for (int j = 0; j < numContacts; j++) {
btManifoldPoint& pt = contactManifold->getContactPoint(j);
// check to see if contact point is touching the bottom sphere of the capsule.
// and the contact normal is not slanted too much.
float contactPointY = (obA == _rigidBody) ? pt.m_localPointA.getY() : pt.m_localPointB.getY();
btVector3 normal = (obA == _rigidBody) ? pt.m_normalWorldOnB : -pt.m_normalWorldOnB;
if (contactPointY < -_halfHeight && normal.dot(_currentUp) > COS_PI_OVER_THREE) {
return true;
// check for "floor"
btManifoldPoint& contact = contactManifold->getContactPoint(j);
btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame
btVector3 normal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character
btScalar hitHeight = _halfHeight + _radius + pointOnCharacter.dot(_currentUp);
if (hitHeight < _maxStepHeight && normal.dot(_currentUp) > _minFloorNormalDotUp) {
hasFloor = true;
if (!pushing) {
// we're not pushing against anything so we can early exit
// (all we need to know is that there is a floor)
break;
}
}
if (pushing && _targetVelocity.dot(normal) < 0.0f) {
// remember highest step obstacle
if (!_stepUpEnabled || hitHeight > _maxStepHeight) {
// this manifold is invalidated by point that is too high
stepContactIndex = -1;
break;
} else if (hitHeight > highestStep && normal.dot(_targetVelocity) < 0.0f ) {
highestStep = hitHeight;
stepContactIndex = j;
hasFloor = true;
}
}
}
if (stepContactIndex > -1 && highestStep > _stepHeight) {
// remember step info for later
btManifoldPoint& contact = contactManifold->getContactPoint(stepContactIndex);
btVector3 pointOnCharacter = characterIsFirst ? contact.m_localPointA : contact.m_localPointB; // object-local-frame
_stepNormal = characterIsFirst ? contact.m_normalWorldOnB : -contact.m_normalWorldOnB; // points toward character
_stepHeight = highestStep;
_stepPoint = rotation * pointOnCharacter; // rotate into world-frame
}
if (hasFloor && !(pushing && _stepUpEnabled)) {
// early exit since all we need to know is that we're on a floor
break;
}
}
}
return false;
return hasFloor;
}
void CharacterController::updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) {
_preActionVelocity = getLinearVelocity();
preStep(collisionWorld);
playerStep(collisionWorld, deltaTime);
}
void CharacterController::preStep(btCollisionWorld* collisionWorld) {
// trace a ray straight down to see if we're standing on the ground
const btTransform& xform = _rigidBody->getWorldTransform();
const btTransform& transform = _rigidBody->getWorldTransform();
// rayStart is at center of bottom sphere
btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp;
btVector3 rayStart = transform.getOrigin() - _halfHeight * _currentUp;
// rayEnd is some short distance outside bottom sphere
const btScalar FLOOR_PROXIMITY_THRESHOLD = 0.3f * _radius;
@ -183,54 +232,117 @@ void CharacterController::preStep(btCollisionWorld* collisionWorld) {
if (rayCallback.hasHit()) {
_floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius;
}
_hasSupport = checkForSupport(collisionWorld);
}
const btScalar MIN_TARGET_SPEED = 0.001f;
const btScalar MIN_TARGET_SPEED_SQUARED = MIN_TARGET_SPEED * MIN_TARGET_SPEED;
void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
void CharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt) {
_stepHeight = _minStepHeight; // clears memory of last step obstacle
btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity;
computeNewVelocity(dt, velocity);
_rigidBody->setLinearVelocity(velocity + _parentVelocity);
// Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform.
// Rather than add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal.
// This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate().
const float MINIMUM_TIME_REMAINING = 0.005f;
const float MAX_DISPLACEMENT = 0.5f * _radius;
_followTimeRemaining -= dt;
if (_followTimeRemaining >= MINIMUM_TIME_REMAINING) {
if (_following) {
_followTimeAccumulator += dt;
// linear part uses a motor
const float MAX_WALKING_SPEED = 30.5f; // TODO: scale this stuff with avatar size
const float MAX_WALKING_SPEED_DISTANCE = 1.0f;
const float NORMAL_WALKING_SPEED = 0.5f * MAX_WALKING_SPEED;
const float NORMAL_WALKING_SPEED_DISTANCE = 0.5f * MAX_WALKING_SPEED_DISTANCE;
const float FEW_SUBSTEPS = 4.0f * dt;
btTransform bodyTransform = _rigidBody->getWorldTransform();
btVector3 startPos = bodyTransform.getOrigin();
btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;
btVector3 vel = deltaPos / _followTimeRemaining;
btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT); // clamp displacement to prevent tunneling.
btVector3 endPos = startPos + linearDisplacement;
btScalar deltaDistance = deltaPos.length();
const float MIN_DELTA_DISTANCE = 0.01f; // TODO: scale by avatar size but cap at (NORMAL_WALKING_SPEED * FEW_SUBSTEPS)
if (deltaDistance > MIN_DELTA_DISTANCE) {
btVector3 vel = deltaPos;
if (_state == State::Hover) {
btScalar HOVER_FOLLOW_VELOCITY_TIMESCALE = 0.1f;
vel /= HOVER_FOLLOW_VELOCITY_TIMESCALE;
} else {
if (deltaDistance > MAX_WALKING_SPEED_DISTANCE) {
// cap max speed
vel *= MAX_WALKING_SPEED / deltaDistance;
} else if (deltaDistance > NORMAL_WALKING_SPEED_DISTANCE) {
// linearly interpolate to NORMAL_WALKING_SPEED
btScalar alpha = (deltaDistance - NORMAL_WALKING_SPEED_DISTANCE) / (MAX_WALKING_SPEED_DISTANCE - NORMAL_WALKING_SPEED_DISTANCE);
vel *= NORMAL_WALKING_SPEED * (1.0f - alpha) + MAX_WALKING_SPEED * alpha;
} else {
// use exponential decay but cap at NORMAL_WALKING_SPEED
vel /= FEW_SUBSTEPS;
btScalar speed = vel.length();
if (speed > NORMAL_WALKING_SPEED) {
vel *= NORMAL_WALKING_SPEED / speed;
}
}
}
vel += _followVelocity;
const float HORIZONTAL_FOLLOW_TIMESCALE = 0.05f;
float verticalFollowTimescale = 20.0f;
if (_state == State::Hover) {
verticalFollowTimescale = HORIZONTAL_FOLLOW_TIMESCALE;
} else {
// remove vertical component
vel -= vel.dot(_currentUp) * _currentUp;
}
glm::quat worldFrameRotation; // identity
addMotor(bulletToGLM(vel), worldFrameRotation, HORIZONTAL_FOLLOW_TIMESCALE, verticalFollowTimescale);
}
// angular part uses incremental teleports
const float ANGULAR_FOLLOW_TIMESCALE = 0.8f;
const float MAX_ANGULAR_SPEED = (PI / 2.0f) / ANGULAR_FOLLOW_TIMESCALE;
btQuaternion startRot = bodyTransform.getRotation();
glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot));
glm::vec2 currentRight(currentFacing.y, -currentFacing.x);
glm::vec2 currentRight(currentFacing.y, - currentFacing.x);
glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation()));
float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f));
float angularSpeed = deltaAngle / _followTimeRemaining;
float angularSpeed = deltaAngle / FEW_SUBSTEPS;
if (angularSpeed > MAX_ANGULAR_SPEED) {
angularSpeed *= MAX_ANGULAR_SPEED / angularSpeed;
}
float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight));
btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt);
btQuaternion endRot = angularDisplacement * startRot;
// in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account.
btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset);
btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset);
_followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement;
_followAngularDisplacement = angularDisplacement * _followAngularDisplacement;
_rigidBody->setWorldTransform(btTransform(endRot, endPos));
_rigidBody->setWorldTransform(btTransform(endRot, startPos));
}
_hasSupport = checkForSupport(collisionWorld, dt);
computeNewVelocity(dt, velocity);
if (_moveKinematically) {
// KINEMATIC_CONTROLLER_HACK
btTransform transform = _rigidBody->getWorldTransform();
transform.setOrigin(_ghost.getWorldTransform().getOrigin());
_ghost.setWorldTransform(transform);
_ghost.setMotorVelocity(_targetVelocity);
float overshoot = 1.0f * _radius;
_ghost.move(dt, overshoot, _gravity);
transform.setOrigin(_ghost.getWorldTransform().getOrigin());
_rigidBody->setWorldTransform(transform);
_rigidBody->setLinearVelocity(_ghost.getLinearVelocity());
} else {
float stepUpSpeed2 = _stepUpVelocity.length2();
if (stepUpSpeed2 > FLT_EPSILON) {
// we step up with teleports rather than applying velocity
// use a speed that would ballistically reach _stepHeight under gravity
_stepUpVelocity /= sqrtf(stepUpSpeed2);
btScalar minStepUpSpeed = sqrtf(fabsf(2.0f * _gravity * _stepHeight));
btTransform transform = _rigidBody->getWorldTransform();
transform.setOrigin(transform.getOrigin() + (dt * minStepUpSpeed) * _stepUpVelocity);
_rigidBody->setWorldTransform(transform);
// make sure the upward velocity is large enough to clear the very top of the step
const btScalar MAGIC_STEP_OVERSHOOT_SPEED_COEFFICIENT = 0.5f;
minStepUpSpeed = MAGIC_STEP_OVERSHOOT_SPEED_COEFFICIENT * sqrtf(fabsf(2.0f * _gravity * _minStepHeight));
btScalar vDotUp = velocity.dot(_currentUp);
if (vDotUp < minStepUpSpeed) {
velocity += (minStepUpSpeed - vDotUp) * _stepUpVelocity;
}
}
_rigidBody->setLinearVelocity(velocity + _parentVelocity);
_ghost.setWorldTransform(_rigidBody->getWorldTransform());
}
_followTime += dt;
}
void CharacterController::jump() {
@ -272,95 +384,96 @@ void CharacterController::setState(State desiredState) {
#ifdef DEBUG_STATE_CHANGE
qCDebug(physics) << "CharacterController::setState" << stateToStr(desiredState) << "from" << stateToStr(_state) << "," << reason;
#endif
if (desiredState == State::Hover && _state != State::Hover) {
// hover enter
if (_rigidBody) {
if (_rigidBody) {
if (desiredState == State::Hover && _state != State::Hover) {
// hover enter
_rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
}
} else if (_state == State::Hover && desiredState != State::Hover) {
// hover exit
if (_rigidBody) {
_rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
} else if (_state == State::Hover && desiredState != State::Hover) {
// hover exit
if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS) {
_rigidBody->setGravity(btVector3(0.0f, 0.0f, 0.0f));
} else {
_rigidBody->setGravity(_gravity * _currentUp);
}
}
}
_state = desiredState;
}
}
void CharacterController::setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale) {
_boxScale = scale;
float x = _boxScale.x;
float z = _boxScale.z;
void CharacterController::setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale) {
float x = scale.x;
float z = scale.z;
float radius = 0.5f * sqrtf(0.5f * (x * x + z * z));
float halfHeight = 0.5f * _boxScale.y - radius;
float halfHeight = 0.5f * scale.y - radius;
float MIN_HALF_HEIGHT = 0.1f;
if (halfHeight < MIN_HALF_HEIGHT) {
halfHeight = MIN_HALF_HEIGHT;
}
// compare dimensions
float radiusDelta = glm::abs(radius - _radius);
float heightDelta = glm::abs(halfHeight - _halfHeight);
if (radiusDelta < FLT_EPSILON && heightDelta < FLT_EPSILON) {
// shape hasn't changed --> nothing to do
} else {
if (glm::abs(radius - _radius) > FLT_EPSILON || glm::abs(halfHeight - _halfHeight) > FLT_EPSILON) {
_radius = radius;
_halfHeight = halfHeight;
const btScalar DEFAULT_MIN_STEP_HEIGHT = 0.041f; // HACK: hardcoded now but should just larger than shape margin
const btScalar MAX_STEP_FRACTION_OF_HALF_HEIGHT = 0.56f;
_minStepHeight = DEFAULT_MIN_STEP_HEIGHT;
_maxStepHeight = MAX_STEP_FRACTION_OF_HALF_HEIGHT * (_halfHeight + _radius);
if (_dynamicsWorld) {
// must REMOVE from world prior to shape update
_pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;
}
_pendingFlags |= PENDING_FLAG_UPDATE_SHAPE;
// only need to ADD back when we happen to be enabled
if (_enabled) {
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
}
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
}
// it's ok to change offset immediately -- there are no thread safety issues here
_shapeLocalOffset = corner + 0.5f * _boxScale;
_shapeLocalOffset = minCorner + 0.5f * scale;
}
void CharacterController::setEnabled(bool enabled) {
if (enabled != _enabled) {
if (enabled) {
// Don't bother clearing REMOVE bit since it might be paired with an UPDATE_SHAPE bit.
// Setting the ADD bit here works for all cases so we don't even bother checking other bits.
_pendingFlags |= PENDING_FLAG_ADD_TO_SIMULATION;
} else {
if (_dynamicsWorld) {
_pendingFlags |= PENDING_FLAG_REMOVE_FROM_SIMULATION;
}
_pendingFlags &= ~ PENDING_FLAG_ADD_TO_SIMULATION;
void CharacterController::setCollisionGroup(int16_t group) {
if (_collisionGroup != group) {
_collisionGroup = group;
_pendingFlags |= PENDING_FLAG_UPDATE_COLLISION_GROUP;
_ghost.setCollisionGroupAndMask(_collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR & (~ _collisionGroup));
}
}
void CharacterController::handleChangedCollisionGroup() {
if (_pendingFlags & PENDING_FLAG_UPDATE_COLLISION_GROUP) {
// ATM the easiest way to update collision groups is to remove/re-add the RigidBody
if (_dynamicsWorld) {
_dynamicsWorld->removeRigidBody(_rigidBody);
_dynamicsWorld->addRigidBody(_rigidBody, _collisionGroup, BULLET_COLLISION_MASK_MY_AVATAR);
}
_pendingFlags &= ~PENDING_FLAG_UPDATE_COLLISION_GROUP;
if (_state != State::Hover && _rigidBody) {
_gravity = _collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS ? 0.0f : DEFAULT_CHARACTER_GRAVITY;
_rigidBody->setGravity(_gravity * _currentUp);
}
SET_STATE(State::Hover, "setEnabled");
_enabled = enabled;
}
}
void CharacterController::updateUpAxis(const glm::quat& rotation) {
btVector3 oldUp = _currentUp;
_currentUp = quatRotate(glmToBullet(rotation), LOCAL_UP_AXIS);
if (_state != State::Hover) {
const btScalar MIN_UP_ERROR = 0.01f;
if (oldUp.distance(_currentUp) > MIN_UP_ERROR) {
_rigidBody->setGravity(DEFAULT_CHARACTER_GRAVITY * _currentUp);
}
_ghost.setUpDirection(_currentUp);
if (_state != State::Hover && _rigidBody) {
_rigidBody->setGravity(_gravity * _currentUp);
}
}
void CharacterController::setPositionAndOrientation(
const glm::vec3& position,
const glm::quat& orientation) {
// TODO: update gravity if up has changed
updateUpAxis(orientation);
btQuaternion bodyOrientation = glmToBullet(orientation);
btVector3 bodyPosition = glmToBullet(position + orientation * _shapeLocalOffset);
_characterBodyTransform = btTransform(bodyOrientation, bodyPosition);
_rotation = glmToBullet(orientation);
_position = glmToBullet(position + orientation * _shapeLocalOffset);
}
void CharacterController::getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const {
if (_enabled && _rigidBody) {
if (_rigidBody) {
const btTransform& avatarTransform = _rigidBody->getWorldTransform();
rotation = bulletToGLM(avatarTransform.getRotation());
position = bulletToGLM(avatarTransform.getOrigin()) - rotation * _shapeLocalOffset;
@ -371,25 +484,43 @@ void CharacterController::setParentVelocity(const glm::vec3& velocity) {
_parentVelocity = glmToBullet(velocity);
}
void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix, float timeRemaining) {
_followTimeRemaining = timeRemaining;
void CharacterController::setFollowParameters(const glm::mat4& desiredWorldBodyMatrix) {
_followDesiredBodyTransform = glmToBullet(desiredWorldBodyMatrix) * btTransform(btQuaternion::getIdentity(), glmToBullet(_shapeLocalOffset));
}
if (!_following) {
_following = true;
_followVelocity = btVector3(0.0f, 0.0f, 0.0f);
} else if (_followTimeAccumulator > 0.0f) {
btVector3 newFollowVelocity = (_followDesiredBodyTransform.getOrigin() - _previousFollowPosition) / _followTimeAccumulator;
const float dontDivideByZero = 0.001f;
float newSpeed = newFollowVelocity.length() + dontDivideByZero;
float oldSpeed = _followVelocity.length();
glm::vec3 CharacterController::getFollowLinearDisplacement() const {
return bulletToGLM(_followLinearDisplacement);
}
bool successfulSnap = false;
btVector3 offset = _followDesiredBodyTransform.getOrigin() - _position;
const btScalar MAX_FOLLOW_OFFSET = 10.0f * _radius;
if (offset.length() > MAX_FOLLOW_OFFSET) {
successfulSnap = queryPenetration(_followDesiredBodyTransform);
if (successfulSnap) {
_position = _followDesiredBodyTransform.getOrigin();
}
}
glm::quat CharacterController::getFollowAngularDisplacement() const {
return bulletToGLM(_followAngularDisplacement);
}
glm::vec3 CharacterController::getFollowVelocity() const {
if (_followTime > 0.0f) {
return bulletToGLM(_followLinearDisplacement) / _followTime;
} else {
return glm::vec3();
const float VERY_SLOW_HOVER_SPEED = 0.25f;
const float FAST_CHANGE_SPEED_RATIO = 100.0f;
if (successfulSnap || (oldSpeed / newSpeed > FAST_CHANGE_SPEED_RATIO && newSpeed < VERY_SLOW_HOVER_SPEED)) {
// character is snapping to avatar position or avatar is stopping quickly
// HACK: slam _followVelocity and _rigidBody velocity immediately
_followVelocity = newFollowVelocity;
newFollowVelocity.setY(0.0f);
_rigidBody->setLinearVelocity(newFollowVelocity);
} else {
// use simple blending to filter noise from the velocity measurement
const float blend = 0.2f;
_followVelocity = (1.0f - blend) * _followVelocity + blend * newFollowVelocity;
}
}
_previousFollowPosition = _followDesiredBodyTransform.getOrigin();
_followTimeAccumulator = 0.0f;
}
glm::vec3 CharacterController::getLinearVelocity() const {
@ -400,6 +531,10 @@ glm::vec3 CharacterController::getLinearVelocity() const {
return velocity;
}
glm::vec3 CharacterController::getPreActionLinearVelocity() const {
return _preActionVelocity;
}
glm::vec3 CharacterController::getVelocityChange() const {
if (_rigidBody) {
return bulletToGLM(_velocityChange);
@ -428,16 +563,18 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
btScalar angle = motor.rotation.getAngle();
btVector3 velocity = worldVelocity.rotate(axis, -angle);
if (_state == State::Hover || motor.hTimescale == motor.vTimescale) {
if (_collisionGroup == BULLET_COLLISION_GROUP_COLLISIONLESS ||
_state == State::Hover || motor.hTimescale == motor.vTimescale) {
// modify velocity
btScalar tau = dt / motor.hTimescale;
if (tau > 1.0f) {
tau = 1.0f;
}
velocity += (motor.velocity - velocity) * tau;
velocity += tau * (motor.velocity - velocity);
// rotate back into world-frame
velocity = velocity.rotate(axis, angle);
_targetVelocity += (tau * motor.velocity).rotate(axis, angle);
// store the velocity and weight
velocities.push_back(velocity);
@ -445,12 +582,32 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
} else {
// compute local UP
btVector3 up = _currentUp.rotate(axis, -angle);
btVector3 motorVelocity = motor.velocity;
// save these non-adjusted components for later
btVector3 vTargetVelocity = motorVelocity.dot(up) * up;
btVector3 hTargetVelocity = motorVelocity - vTargetVelocity;
if (_stepHeight > _minStepHeight) {
// there is a step --> compute velocity direction to go over step
btVector3 motorVelocityWF = motorVelocity.rotate(axis, angle);
if (motorVelocityWF.dot(_stepNormal) < 0.0f) {
// the motor pushes against step
motorVelocityWF = _stepNormal.cross(_stepPoint.cross(motorVelocityWF));
btScalar doubleCrossLength2 = motorVelocityWF.length2();
if (doubleCrossLength2 > FLT_EPSILON) {
// scale the motor in the correct direction and rotate back to motor-frame
motorVelocityWF *= (motorVelocity.length() / sqrtf(doubleCrossLength2));
_stepUpVelocity += motorVelocityWF.rotate(axis, -angle);
}
}
}
// split velocity into horizontal and vertical components
btVector3 vVelocity = velocity.dot(up) * up;
btVector3 hVelocity = velocity - vVelocity;
btVector3 vTargetVelocity = motor.velocity.dot(up) * up;
btVector3 hTargetVelocity = motor.velocity - vTargetVelocity;
btVector3 vMotorVelocity = motorVelocity.dot(up) * up;
btVector3 hMotorVelocity = motorVelocity - vMotorVelocity;
// modify each component separately
btScalar maxTau = 0.0f;
@ -460,7 +617,7 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
tau = 1.0f;
}
maxTau = tau;
hVelocity += (hTargetVelocity - hVelocity) * tau;
hVelocity += (hMotorVelocity - hVelocity) * tau;
}
if (motor.vTimescale < MAX_CHARACTER_MOTOR_TIMESCALE) {
btScalar tau = dt / motor.vTimescale;
@ -470,11 +627,12 @@ void CharacterController::applyMotor(int index, btScalar dt, btVector3& worldVel
if (tau > maxTau) {
maxTau = tau;
}
vVelocity += (vTargetVelocity - vVelocity) * tau;
vVelocity += (vMotorVelocity - vVelocity) * tau;
}
// add components back together and rotate into world-frame
velocity = (hVelocity + vVelocity).rotate(axis, angle);
_targetVelocity += maxTau * (hTargetVelocity + vTargetVelocity).rotate(axis, angle);
// store velocity and weights
velocities.push_back(velocity);
@ -492,6 +650,8 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) {
velocities.reserve(_motors.size());
std::vector<btScalar> weights;
weights.reserve(_motors.size());
_targetVelocity = btVector3(0.0f, 0.0f, 0.0f);
_stepUpVelocity = btVector3(0.0f, 0.0f, 0.0f);
for (int i = 0; i < (int)_motors.size(); ++i) {
applyMotor(i, dt, velocity, velocities, weights);
}
@ -507,14 +667,18 @@ void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) {
for (size_t i = 0; i < velocities.size(); ++i) {
velocity += (weights[i] / totalWeight) * velocities[i];
}
_targetVelocity /= totalWeight;
}
if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) {
velocity = btVector3(0.0f, 0.0f, 0.0f);
}
// 'thrust' is applied at the very end
_targetVelocity += dt * _linearAcceleration;
velocity += dt * _linearAcceleration;
_targetVelocity = velocity;
// Note the differences between these two variables:
// _targetVelocity = ideal final velocity according to input
// velocity = real final velocity after motors are applied to current velocity
}
void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) {
@ -523,57 +687,54 @@ void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) {
velocity = bulletToGLM(btVelocity);
}
void CharacterController::preSimulation() {
if (_enabled && _dynamicsWorld && _rigidBody) {
quint64 now = usecTimestampNow();
void CharacterController::updateState() {
if (!_dynamicsWorld) {
return;
}
const btScalar FLY_TO_GROUND_THRESHOLD = 0.1f * _radius;
const btScalar GROUND_TO_FLY_THRESHOLD = 0.8f * _radius + _halfHeight;
const quint64 TAKE_OFF_TO_IN_AIR_PERIOD = 250 * MSECS_PER_SECOND;
const btScalar MIN_HOVER_HEIGHT = 2.5f;
const quint64 JUMP_TO_HOVER_PERIOD = 1100 * MSECS_PER_SECOND;
// slam body to where it is supposed to be
_rigidBody->setWorldTransform(_characterBodyTransform);
btVector3 velocity = _rigidBody->getLinearVelocity();
_preSimulationVelocity = velocity;
// scan for distant floor
// rayStart is at center of bottom sphere
btVector3 rayStart = _position;
// scan for distant floor
// rayStart is at center of bottom sphere
btVector3 rayStart = _characterBodyTransform.getOrigin();
// rayEnd is straight down MAX_FALL_HEIGHT
btScalar rayLength = _radius + MAX_FALL_HEIGHT;
btVector3 rayEnd = rayStart - rayLength * _currentUp;
// rayEnd is straight down MAX_FALL_HEIGHT
btScalar rayLength = _radius + MAX_FALL_HEIGHT;
btVector3 rayEnd = rayStart - rayLength * _currentUp;
const btScalar FLY_TO_GROUND_THRESHOLD = 0.1f * _radius;
const btScalar GROUND_TO_FLY_THRESHOLD = 0.8f * _radius + _halfHeight;
const quint64 TAKE_OFF_TO_IN_AIR_PERIOD = 250 * MSECS_PER_SECOND;
const btScalar MIN_HOVER_HEIGHT = 2.5f;
const quint64 JUMP_TO_HOVER_PERIOD = 1100 * MSECS_PER_SECOND;
const btScalar MAX_WALKING_SPEED = 2.5f;
ClosestNotMe rayCallback(_rigidBody);
rayCallback.m_closestHitFraction = 1.0f;
_dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback);
bool rayHasHit = rayCallback.hasHit();
quint64 now = usecTimestampNow();
if (rayHasHit) {
_rayHitStartTime = now;
_floorDistance = rayLength * rayCallback.m_closestHitFraction - (_radius + _halfHeight);
} else {
const quint64 RAY_HIT_START_PERIOD = 500 * MSECS_PER_SECOND;
ClosestNotMe rayCallback(_rigidBody);
rayCallback.m_closestHitFraction = 1.0f;
_dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback);
bool rayHasHit = rayCallback.hasHit();
if (rayHasHit) {
_rayHitStartTime = now;
_floorDistance = rayLength * rayCallback.m_closestHitFraction - (_radius + _halfHeight);
} else if ((now - _rayHitStartTime) < RAY_HIT_START_PERIOD) {
if ((now - _rayHitStartTime) < RAY_HIT_START_PERIOD) {
rayHasHit = true;
} else {
_floorDistance = FLT_MAX;
}
}
// record a time stamp when the jump button was first pressed.
if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) {
if (_pendingFlags & PENDING_FLAG_JUMP) {
_jumpButtonDownStartTime = now;
_jumpButtonDownCount++;
}
// record a time stamp when the jump button was first pressed.
bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP;
if ((_previousFlags & PENDING_FLAG_JUMP) != (_pendingFlags & PENDING_FLAG_JUMP)) {
if (_pendingFlags & PENDING_FLAG_JUMP) {
_jumpButtonDownStartTime = now;
_jumpButtonDownCount++;
}
}
bool jumpButtonHeld = _pendingFlags & PENDING_FLAG_JUMP;
btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp;
bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f);
btVector3 velocity = _preSimulationVelocity;
// OUTOFBODY_HACK -- disable normal state transitions while collisionless
if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) {
switch (_state) {
case State::Ground:
if (!rayHasHit && !_hasSupport) {
@ -613,32 +774,50 @@ void CharacterController::preSimulation() {
break;
}
case State::Hover:
if ((_floorDistance < MIN_HOVER_HEIGHT) && !jumpButtonHeld && !flyingFast) {
btVector3 actualHorizVelocity = velocity - velocity.dot(_currentUp) * _currentUp;
const btScalar MAX_WALKING_SPEED = 2.5f;
bool flyingFast = _state == State::Hover && actualHorizVelocity.length() > (MAX_WALKING_SPEED * 0.75f);
if ((_floorDistance < MIN_HOVER_HEIGHT) &&
!(jumpButtonHeld || flyingFast || (now - _jumpButtonDownStartTime) > JUMP_TO_HOVER_PERIOD)) {
SET_STATE(State::InAir, "near ground");
} else if (((_floorDistance < FLY_TO_GROUND_THRESHOLD) || _hasSupport) && !flyingFast) {
SET_STATE(State::Ground, "touching ground");
}
break;
}
if (_moveKinematically && _ghost.isHovering()) {
SET_STATE(State::Hover, "kinematic motion"); // HACK
}
} else {
// OUTOFBODY_HACK -- in collisionless state switch only between Ground and Hover states
if (rayHasHit) {
SET_STATE(State::Ground, "collisionless above ground");
} else {
SET_STATE(State::Hover, "collisionless in air");
}
}
}
void CharacterController::preSimulation() {
if (_rigidBody) {
// slam body transform and remember velocity
_rigidBody->setWorldTransform(btTransform(btTransform(_rotation, _position)));
_preSimulationVelocity = _rigidBody->getLinearVelocity();
updateState();
}
_previousFlags = _pendingFlags;
_pendingFlags &= ~PENDING_FLAG_JUMP;
_followTime = 0.0f;
_followLinearDisplacement = btVector3(0, 0, 0);
_followAngularDisplacement = btQuaternion::getIdentity();
}
void CharacterController::postSimulation() {
// postSimulation() exists for symmetry and just in case we need to do something here later
if (_enabled && _dynamicsWorld && _rigidBody) {
btVector3 velocity = _rigidBody->getLinearVelocity();
_velocityChange = velocity - _preSimulationVelocity;
if (_rigidBody) {
_velocityChange = _rigidBody->getLinearVelocity() - _preSimulationVelocity;
}
}
bool CharacterController::getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) {
if (!_rigidBody) {
return false;
@ -655,7 +834,58 @@ void CharacterController::setFlyingAllowed(bool value) {
_flyingAllowed = value;
if (!_flyingAllowed && _state == State::Hover) {
SET_STATE(State::InAir, "flying not allowed");
// OUTOFBODY_HACK -- disable normal state transitions while collisionless
if (_collisionGroup == BULLET_COLLISION_GROUP_MY_AVATAR) {
SET_STATE(State::InAir, "flying not allowed");
}
}
}
}
float CharacterController::measureMaxHipsOffsetRadius(const glm::vec3& currentHipsOffset, float maxSweepDistance) {
btVector3 hipsOffset = glmToBullet(currentHipsOffset); // rig-frame
btScalar hipsOffsetLength = hipsOffset.length();
if (hipsOffsetLength > FLT_EPSILON) {
const btTransform& transform = _rigidBody->getWorldTransform();
// rotate into world-frame
btTransform rotation = transform;
rotation.setOrigin(btVector3(0.0f, 0.0f, 0.0f));
btVector3 startPos = transform.getOrigin() - rotation * glmToBullet(_shapeLocalOffset);
btTransform startTransform = transform;
startTransform.setOrigin(startPos);
btVector3 endPos = startPos + rotation * ((maxSweepDistance / hipsOffsetLength) * hipsOffset);
// sweep test a sphere
btSphereShape sphere(_radius);
CharacterSweepResult result(&_ghost);
btTransform endTransform = startTransform;
endTransform.setOrigin(endPos);
_ghost.sweepTest(&sphere, startTransform, endTransform, result);
// measure sweep success
if (result.hasHit()) {
maxSweepDistance *= result.m_closestHitFraction;
}
}
return maxSweepDistance;
}
void CharacterController::setMoveKinematically(bool kinematic) {
if (kinematic != _moveKinematically) {
_moveKinematically = kinematic;
_pendingFlags |= PENDING_FLAG_UPDATE_SHAPE;
_ghost.setMotorOnly(!_moveKinematically);
}
}
bool CharacterController::queryPenetration(const btTransform& transform) {
btVector3 minBox;
btVector3 maxBox;
_ghost.setWorldTransform(transform);
_ghost.measurePenetration(minBox, maxBox);
btVector3 penetration = minBox;
penetration.setMax(maxBox.absolute());
const btScalar MIN_PENETRATION_SQUARED = 0.0016f; // 0.04^2
return penetration.length2() < MIN_PENETRATION_SQUARED;
}

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

@ -1,6 +1,6 @@
//
// CharacterControllerInterface.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2015.10.21
// Copyright 2015 High Fidelity, Inc.
@ -9,8 +9,8 @@
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#ifndef hifi_CharacterControllerInterface_h
#define hifi_CharacterControllerInterface_h
#ifndef hifi_CharacterController_h
#define hifi_CharacterController_h
#include <assert.h>
#include <stdint.h>
@ -19,12 +19,18 @@
#include <BulletDynamics/Character/btCharacterControllerInterface.h>
#include <GLMHelpers.h>
#include <NumericalConstants.h>
#include <PhysicsCollisionGroups.h>
#include "BulletUtil.h"
#include "CharacterGhostObject.h"
const uint32_t PENDING_FLAG_ADD_TO_SIMULATION = 1U << 0;
const uint32_t PENDING_FLAG_REMOVE_FROM_SIMULATION = 1U << 1;
const uint32_t PENDING_FLAG_UPDATE_SHAPE = 1U << 2;
const uint32_t PENDING_FLAG_JUMP = 1U << 3;
const uint32_t PENDING_FLAG_UPDATE_COLLISION_GROUP = 1U << 4;
const float DEFAULT_MIN_FLOOR_NORMAL_DOT_UP = cosf(PI / 3.0f);
const float DEFAULT_CHARACTER_GRAVITY = -5.0f;
@ -44,7 +50,7 @@ public:
bool needsRemoval() const;
bool needsAddition() const;
void setDynamicsWorld(btDynamicsWorld* world);
virtual void setDynamicsWorld(btDynamicsWorld* world);
btCollisionObject* getCollisionObject() { return _rigidBody; }
virtual void updateShapeIfNecessary() = 0;
@ -56,10 +62,7 @@ public:
virtual void warp(const btVector3& origin) override { }
virtual void debugDraw(btIDebugDraw* debugDrawer) override { }
virtual void setUpInterpolate(bool value) override { }
virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override {
preStep(collisionWorld);
playerStep(collisionWorld, deltaTime);
}
virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTime) override;
virtual void preStep(btCollisionWorld *collisionWorld) override;
virtual void playerStep(btCollisionWorld *collisionWorld, btScalar dt) override;
virtual bool canJump() const override { assert(false); return false; } // never call this
@ -69,6 +72,7 @@ public:
void clearMotors();
void addMotor(const glm::vec3& velocity, const glm::quat& rotation, float horizTimescale, float vertTimescale = -1.0f);
void applyMotor(int index, btScalar dt, btVector3& worldVelocity, std::vector<btVector3>& velocities, std::vector<btScalar>& weights);
void setStepUpEnabled(bool enabled) { _stepUpEnabled = enabled; }
void computeNewVelocity(btScalar dt, btVector3& velocity);
void computeNewVelocity(btScalar dt, glm::vec3& velocity);
@ -82,13 +86,11 @@ public:
void getPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const;
void setParentVelocity(const glm::vec3& parentVelocity);
void setFollowParameters(const glm::mat4& desiredWorldMatrix, float timeRemaining);
float getFollowTime() const { return _followTime; }
glm::vec3 getFollowLinearDisplacement() const;
glm::quat getFollowAngularDisplacement() const;
glm::vec3 getFollowVelocity() const;
void setFollowParameters(const glm::mat4& desiredWorldBodyMatrix);
void disableFollow() { _following = false; }
glm::vec3 getLinearVelocity() const;
glm::vec3 getPreActionLinearVelocity() const;
glm::vec3 getVelocityChange() const;
float getCapsuleRadius() const { return _radius; }
@ -103,17 +105,24 @@ public:
};
State getState() const { return _state; }
void updateState();
void setLocalBoundingBox(const glm::vec3& corner, const glm::vec3& scale);
void setLocalBoundingBox(const glm::vec3& minCorner, const glm::vec3& scale);
bool isEnabled() const { return _enabled; } // thread-safe
void setEnabled(bool enabled);
bool isEnabledAndReady() const { return _enabled && _dynamicsWorld; }
bool isEnabledAndReady() const { return _dynamicsWorld; }
void setCollisionGroup(int16_t group);
int16_t getCollisionGroup() const { return _collisionGroup; }
void handleChangedCollisionGroup();
bool getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation);
void setFlyingAllowed(bool value);
float measureMaxHipsOffsetRadius(const glm::vec3& currentHipsOffset, float maxSweepDistance);
void setMoveKinematically(bool kinematic); // KINEMATIC_CONTROLLER_HACK
bool queryPenetration(const btTransform& transform);
protected:
#ifdef DEBUG_STATE_CHANGE
@ -123,7 +132,7 @@ protected:
#endif
void updateUpAxis(const glm::quat& rotation);
bool checkForSupport(btCollisionWorld* collisionWorld) const;
bool checkForSupport(btCollisionWorld* collisionWorld, btScalar dt);
protected:
struct CharacterMotor {
@ -136,14 +145,18 @@ protected:
};
std::vector<CharacterMotor> _motors;
CharacterGhostObject _ghost; // KINEMATIC_CONTROLLER_HACK
btVector3 _currentUp;
btVector3 _targetVelocity;
btVector3 _parentVelocity;
btVector3 _preSimulationVelocity;
glm::vec3 _preActionVelocity;
btVector3 _velocityChange;
btTransform _followDesiredBodyTransform;
btScalar _followTimeRemaining;
btTransform _characterBodyTransform;
btVector3 _followVelocity { 0.0f, 0.0f, 0.0f };
btVector3 _previousFollowPosition { 0.0f, 0.0f, 0.0f };
btVector3 _position;
btQuaternion _rotation;
glm::vec3 _shapeLocalOffset;
@ -155,21 +168,29 @@ protected:
quint32 _jumpButtonDownCount;
quint32 _takeoffJumpButtonID;
btScalar _halfHeight;
btScalar _radius;
// data for walking up steps
btVector3 _stepPoint { 0.0f, 0.0f, 0.0f };
btVector3 _stepNormal { 0.0f, 0.0f, 0.0f };
btVector3 _stepUpVelocity { 0.0f, 0.0f, 0.0f };
btScalar _stepHeight { 0.0f };
btScalar _minStepHeight { 0.0f };
btScalar _maxStepHeight { 0.0f };
btScalar _minFloorNormalDotUp { DEFAULT_MIN_FLOOR_NORMAL_DOT_UP };
btScalar _halfHeight { 0.0f };
btScalar _radius { 0.0f };
btScalar _floorDistance;
bool _stepUpEnabled { true };
bool _hasSupport;
btScalar _gravity;
btScalar _gravity { DEFAULT_CHARACTER_GRAVITY };
btScalar _followTimeAccumulator { 0.0f };
btScalar _jumpSpeed;
btScalar _followTime;
btVector3 _followLinearDisplacement;
btQuaternion _followAngularDisplacement;
btVector3 _linearAcceleration;
bool _following { false };
std::atomic_bool _enabled;
State _state;
bool _isPushingUp;
@ -179,6 +200,8 @@ protected:
uint32_t _previousFlags { 0 };
bool _flyingAllowed { true };
bool _moveKinematically { false }; // KINEMATIC_CONTROLLER_HACK
int16_t _collisionGroup { BULLET_COLLISION_GROUP_MY_AVATAR };
};
#endif // hifi_CharacterControllerInterface_h
#endif // hifi_CharacterController_h

415
libraries/physics/src/CharacterGhostObject.cpp Executable file
View file

@ -0,0 +1,415 @@
//
// CharacterGhostObject.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2016.08.26
// Copyright 2016 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 "CharacterGhostObject.h"
#include <stdint.h>
#include <assert.h>
#include <PhysicsHelpers.h>
#include "CharacterRayResult.h"
#include "CharacterGhostShape.h"
CharacterGhostObject::~CharacterGhostObject() {
removeFromWorld();
if (_ghostShape) {
delete _ghostShape;
_ghostShape = nullptr;
setCollisionShape(nullptr);
}
}
void CharacterGhostObject::setCollisionGroupAndMask(int16_t group, int16_t mask) {
_collisionFilterGroup = group;
_collisionFilterMask = mask;
// TODO: if this probe is in the world reset ghostObject overlap cache
}
void CharacterGhostObject::getCollisionGroupAndMask(int16_t& group, int16_t& mask) const {
group = _collisionFilterGroup;
mask = _collisionFilterMask;
}
void CharacterGhostObject::setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight) {
_radius = radius;
_halfHeight = halfHeight;
}
void CharacterGhostObject::setUpDirection(const btVector3& up) {
btScalar length = up.length();
if (length > FLT_EPSILON) {
_upDirection /= length;
} else {
_upDirection = btVector3(0.0f, 1.0f, 0.0f);
}
}
void CharacterGhostObject::setMotorVelocity(const btVector3& velocity) {
_motorVelocity = velocity;
if (_motorOnly) {
_linearVelocity = _motorVelocity;
}
}
// override of btCollisionObject::setCollisionShape()
void CharacterGhostObject::setCharacterShape(btConvexHullShape* shape) {
assert(shape);
// we create our own shape with an expanded Aabb for more reliable sweep tests
if (_ghostShape) {
delete _ghostShape;
}
_ghostShape = new CharacterGhostShape(static_cast<const btConvexHullShape*>(shape));
setCollisionShape(_ghostShape);
}
void CharacterGhostObject::setCollisionWorld(btCollisionWorld* world) {
if (world != _world) {
removeFromWorld();
_world = world;
addToWorld();
}
}
void CharacterGhostObject::move(btScalar dt, btScalar overshoot, btScalar gravity) {
bool oldOnFloor = _onFloor;
_onFloor = false;
_steppingUp = false;
assert(_world && _inWorld);
updateVelocity(dt, gravity);
// resolve any penetrations before sweeping
int32_t MAX_LOOPS = 4;
int32_t numExtractions = 0;
btVector3 totalPosition(0.0f, 0.0f, 0.0f);
while (numExtractions < MAX_LOOPS) {
if (resolvePenetration(numExtractions)) {
numExtractions = 0;
break;
}
totalPosition += getWorldTransform().getOrigin();
++numExtractions;
}
if (numExtractions > 1) {
// penetration resolution was probably oscillating between opposing objects
// so we use the average of the solutions
totalPosition /= btScalar(numExtractions);
btTransform transform = getWorldTransform();
transform.setOrigin(totalPosition);
setWorldTransform(transform);
// TODO: figure out how to untrap character
}
btTransform startTransform = getWorldTransform();
btVector3 startPosition = startTransform.getOrigin();
if (_onFloor) {
// resolvePenetration() pushed the avatar out of a floor so
// we must updateTraction() before using _linearVelocity
updateTraction(startPosition);
}
btScalar speed = _linearVelocity.length();
btVector3 forwardSweep = dt * _linearVelocity;
btScalar stepDistance = dt * speed;
btScalar MIN_SWEEP_DISTANCE = 0.0001f;
if (stepDistance < MIN_SWEEP_DISTANCE) {
// not moving, no need to sweep
updateTraction(startPosition);
return;
}
// augment forwardSweep to help slow moving sweeps get over steppable ledges
const btScalar MIN_OVERSHOOT = 0.04f; // default margin
if (overshoot < MIN_OVERSHOOT) {
overshoot = MIN_OVERSHOOT;
}
btScalar longSweepDistance = stepDistance + overshoot;
forwardSweep *= longSweepDistance / stepDistance;
// step forward
CharacterSweepResult result(this);
btTransform nextTransform = startTransform;
nextTransform.setOrigin(startPosition + forwardSweep);
sweepTest(_characterShape, startTransform, nextTransform, result); // forward
if (!result.hasHit()) {
nextTransform.setOrigin(startPosition + (stepDistance / longSweepDistance) * forwardSweep);
setWorldTransform(nextTransform);
updateTraction(nextTransform.getOrigin());
return;
}
bool verticalOnly = btFabs(btFabs(_linearVelocity.dot(_upDirection)) - speed) < MIN_OVERSHOOT;
if (verticalOnly) {
// no need to step
nextTransform.setOrigin(startPosition + (result.m_closestHitFraction * stepDistance / longSweepDistance) * forwardSweep);
setWorldTransform(nextTransform);
if (result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) {
_floorNormal = result.m_hitNormalWorld;
_floorContact = result.m_hitPointWorld;
_steppingUp = false;
_onFloor = true;
_hovering = false;
}
updateTraction(nextTransform.getOrigin());
return;
}
// check if this hit is obviously unsteppable
btVector3 hitFromBase = result.m_hitPointWorld - (startPosition - ((_radius + _halfHeight) * _upDirection));
btScalar hitHeight = hitFromBase.dot(_upDirection);
if (hitHeight > _maxStepHeight) {
// shape can't step over the obstacle so move forward as much as possible before we bail
btVector3 forwardTranslation = result.m_closestHitFraction * forwardSweep;
btScalar forwardDistance = forwardTranslation.length();
if (forwardDistance > stepDistance) {
forwardTranslation *= stepDistance / forwardDistance;
}
nextTransform.setOrigin(startPosition + forwardTranslation);
setWorldTransform(nextTransform);
_onFloor = _onFloor || oldOnFloor;
return;
}
// if we get here then we hit something that might be steppable
// remember the forward sweep hit fraction for later
btScalar forwardSweepHitFraction = result.m_closestHitFraction;
// figure out how high we can step up
btScalar availableStepHeight = measureAvailableStepHeight();
// raise by availableStepHeight before sweeping forward
result.resetHitHistory();
startTransform.setOrigin(startPosition + availableStepHeight * _upDirection);
nextTransform.setOrigin(startTransform.getOrigin() + forwardSweep);
sweepTest(_characterShape, startTransform, nextTransform, result);
if (result.hasHit()) {
startTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * forwardSweep);
} else {
startTransform = nextTransform;
}
// sweep down in search of future landing spot
result.resetHitHistory();
btVector3 downSweep = (- availableStepHeight) * _upDirection;
nextTransform.setOrigin(startTransform.getOrigin() + downSweep);
sweepTest(_characterShape, startTransform, nextTransform, result);
if (result.hasHit() && result.m_hitNormalWorld.dot(_upDirection) > _maxWallNormalUpComponent) {
// can stand on future landing spot, so we interpolate toward it
_floorNormal = result.m_hitNormalWorld;
_floorContact = result.m_hitPointWorld;
_steppingUp = true;
_onFloor = true;
_hovering = false;
nextTransform.setOrigin(startTransform.getOrigin() + result.m_closestHitFraction * downSweep);
btVector3 totalStep = nextTransform.getOrigin() - startPosition;
nextTransform.setOrigin(startPosition + (stepDistance / totalStep.length()) * totalStep);
updateTraction(nextTransform.getOrigin());
} else {
// either there is no future landing spot, or there is but we can't stand on it
// in any case: we go forward as much as possible
nextTransform.setOrigin(startPosition + forwardSweepHitFraction * (stepDistance / longSweepDistance) * forwardSweep);
_onFloor = _onFloor || oldOnFloor;
updateTraction(nextTransform.getOrigin());
}
setWorldTransform(nextTransform);
}
bool CharacterGhostObject::sweepTest(
const btConvexShape* shape,
const btTransform& start,
const btTransform& end,
CharacterSweepResult& result) const {
if (_world && _inWorld) {
assert(shape);
btScalar allowedPenetration = _world->getDispatchInfo().m_allowedCcdPenetration;
convexSweepTest(shape, start, end, result, allowedPenetration);
return result.hasHit();
}
return false;
}
bool CharacterGhostObject::rayTest(const btVector3& start,
const btVector3& end,
CharacterRayResult& result) const {
if (_world && _inWorld) {
_world->rayTest(start, end, result);
}
return result.hasHit();
}
void CharacterGhostObject::measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut) {
// minBoxOut and maxBoxOut will be updated with penetration envelope.
// If one of the corner points is <0,0,0> then the penetration is resolvable in a single step,
// but if the space spanned by the two corners extends in both directions along at least one
// component then we the object is sandwiched between two opposing objects.
// We assume this object has just been moved to its current location, or else objects have been
// moved around it since the last step so we must update the overlapping pairs.
refreshOverlappingPairCache();
// compute collision details
btHashedOverlappingPairCache* pairCache = getOverlappingPairCache();
_world->getDispatcher()->dispatchAllCollisionPairs(pairCache, _world->getDispatchInfo(), _world->getDispatcher());
// loop over contact manifolds to compute the penetration box
minBoxOut = btVector3(0.0f, 0.0f, 0.0f);
maxBoxOut = btVector3(0.0f, 0.0f, 0.0f);
btManifoldArray manifoldArray;
int numPairs = pairCache->getNumOverlappingPairs();
for (int i = 0; i < numPairs; i++) {
manifoldArray.resize(0);
btBroadphasePair* collisionPair = &(pairCache->getOverlappingPairArray()[i]);
btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
if ((obj0 && !obj0->hasContactResponse()) && (obj1 && !obj1->hasContactResponse())) {
// we know this probe has no contact response
// but neither does the other object so skip this manifold
continue;
}
if (!collisionPair->m_algorithm) {
// null m_algorithm means the two shape types don't know how to collide!
// shouldn't fall in here but just in case
continue;
}
btScalar mostFloorPenetration = 0.0f;
collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
for (int j = 0; j < manifoldArray.size(); j++) {
btPersistentManifold* manifold = manifoldArray[j];
btScalar directionSign = (manifold->getBody0() == this) ? btScalar(1.0) : btScalar(-1.0);
for (int p = 0; p < manifold->getNumContacts(); p++) {
const btManifoldPoint& pt = manifold->getContactPoint(p);
if (pt.getDistance() > 0.0f) {
continue;
}
// normal always points from object to character
btVector3 normal = directionSign * pt.m_normalWorldOnB;
btScalar penetrationDepth = pt.getDistance();
if (penetrationDepth < mostFloorPenetration) { // remember penetrationDepth is negative
btScalar normalDotUp = normal.dot(_upDirection);
if (normalDotUp > _maxWallNormalUpComponent) {
mostFloorPenetration = penetrationDepth;
_floorNormal = normal;
if (directionSign > 0.0f) {
_floorContact = pt.m_positionWorldOnA;
} else {
_floorContact = pt.m_positionWorldOnB;
}
_onFloor = true;
}
}
btVector3 penetration = (-penetrationDepth) * normal;
minBoxOut.setMin(penetration);
maxBoxOut.setMax(penetration);
}
}
}
}
void CharacterGhostObject::refreshOverlappingPairCache() {
assert(_world && _inWorld);
btVector3 minAabb, maxAabb;
getCollisionShape()->getAabb(getWorldTransform(), minAabb, maxAabb);
// this updates both pairCaches: world broadphase and ghostobject
_world->getBroadphase()->setAabb(getBroadphaseHandle(), minAabb, maxAabb, _world->getDispatcher());
}
void CharacterGhostObject::removeFromWorld() {
if (_world && _inWorld) {
_world->removeCollisionObject(this);
_inWorld = false;
}
}
void CharacterGhostObject::addToWorld() {
if (_world && !_inWorld) {
assert(getCollisionShape());
setCollisionFlags(getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
_world->addCollisionObject(this, _collisionFilterGroup, _collisionFilterMask);
_inWorld = true;
}
}
bool CharacterGhostObject::resolvePenetration(int numTries) {
btVector3 minBox, maxBox;
measurePenetration(minBox, maxBox);
btVector3 restore = maxBox + minBox;
if (restore.length2() > 0.0f) {
btTransform transform = getWorldTransform();
transform.setOrigin(transform.getOrigin() + restore);
setWorldTransform(transform);
return false;
}
return true;
}
void CharacterGhostObject::updateVelocity(btScalar dt, btScalar gravity) {
if (!_motorOnly) {
if (_hovering) {
_linearVelocity *= 0.999f; // HACK damping
} else {
_linearVelocity += (dt * gravity) * _upDirection;
}
}
}
void CharacterGhostObject::updateHoverState(const btVector3& position) {
if (_onFloor) {
_hovering = false;
} else {
// cast a ray down looking for floor support
CharacterRayResult rayResult(this);
btScalar distanceToFeet = _radius + _halfHeight;
btScalar slop = 2.0f * getCollisionShape()->getMargin(); // slop to help ray start OUTSIDE the floor object
btVector3 startPos = position - ((distanceToFeet - slop) * _upDirection);
btVector3 endPos = startPos - (2.0f * distanceToFeet) * _upDirection;
rayTest(startPos, endPos, rayResult);
// we're hovering if the ray didn't hit anything or hit unstandable slope
_hovering = !rayResult.hasHit() || rayResult.m_hitNormalWorld.dot(_upDirection) < _maxWallNormalUpComponent;
}
}
void CharacterGhostObject::updateTraction(const btVector3& position) {
updateHoverState(position);
if (_hovering || _motorOnly) {
_linearVelocity = _motorVelocity;
} else if (_onFloor) {
// compute a velocity that swings the shape around the _floorContact
btVector3 leverArm = _floorContact - position;
btVector3 pathDirection = leverArm.cross(_motorVelocity.cross(leverArm));
btScalar pathLength = pathDirection.length();
if (pathLength > FLT_EPSILON) {
_linearVelocity = (_motorVelocity.length() / pathLength) * pathDirection;
} else {
_linearVelocity = btVector3(0.0f, 0.0f, 0.0f);
}
}
}
btScalar CharacterGhostObject::measureAvailableStepHeight() const {
CharacterSweepResult result(this);
btTransform transform = getWorldTransform();
btTransform nextTransform = transform;
nextTransform.setOrigin(transform.getOrigin() + _maxStepHeight * _upDirection);
sweepTest(_characterShape, transform, nextTransform, result);
return result.m_closestHitFraction * _maxStepHeight;
}

103
libraries/physics/src/CharacterGhostObject.h Executable file
View file

@ -0,0 +1,103 @@
//
// CharacterGhostObject.h
// libraries/physics/src
//
// Created by Andrew Meadows 2016.08.26
// Copyright 2016 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_CharacterGhostObject_h
#define hifi_CharacterGhostObject_h
#include <stdint.h>
#include <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionDispatch/btGhostObject.h>
#include <stdint.h>
#include "CharacterSweepResult.h"
#include "CharacterRayResult.h"
class CharacterGhostShape;
class CharacterGhostObject : public btPairCachingGhostObject {
public:
CharacterGhostObject() { }
~CharacterGhostObject();
void setCollisionGroupAndMask(int16_t group, int16_t mask);
void getCollisionGroupAndMask(int16_t& group, int16_t& mask) const;
void setRadiusAndHalfHeight(btScalar radius, btScalar halfHeight);
void setUpDirection(const btVector3& up);
void setMotorVelocity(const btVector3& velocity);
void setMinWallAngle(btScalar angle) { _maxWallNormalUpComponent = cosf(angle); }
void setMaxStepHeight(btScalar height) { _maxStepHeight = height; }
void setLinearVelocity(const btVector3& velocity) { _linearVelocity = velocity; }
const btVector3& getLinearVelocity() const { return _linearVelocity; }
void setCharacterShape(btConvexHullShape* shape);
void setCollisionWorld(btCollisionWorld* world);
void move(btScalar dt, btScalar overshoot, btScalar gravity);
bool sweepTest(const btConvexShape* shape,
const btTransform& start,
const btTransform& end,
CharacterSweepResult& result) const;
bool rayTest(const btVector3& start,
const btVector3& end,
CharacterRayResult& result) const;
bool isHovering() const { return _hovering; }
void setHovering(bool hovering) { _hovering = hovering; }
void setMotorOnly(bool motorOnly) { _motorOnly = motorOnly; }
bool hasSupport() const { return _onFloor; }
bool isSteppingUp() const { return _steppingUp; }
const btVector3& getFloorNormal() const { return _floorNormal; }
void measurePenetration(btVector3& minBoxOut, btVector3& maxBoxOut);
void refreshOverlappingPairCache();
protected:
void removeFromWorld();
void addToWorld();
bool resolvePenetration(int numTries);
void updateVelocity(btScalar dt, btScalar gravity);
void updateTraction(const btVector3& position);
btScalar measureAvailableStepHeight() const;
void updateHoverState(const btVector3& position);
protected:
btVector3 _upDirection { 0.0f, 1.0f, 0.0f }; // input, up in world-frame
btVector3 _motorVelocity { 0.0f, 0.0f, 0.0f }; // input, velocity character is trying to achieve
btVector3 _linearVelocity { 0.0f, 0.0f, 0.0f }; // internal, actual character velocity
btVector3 _floorNormal { 0.0f, 0.0f, 0.0f }; // internal, probable floor normal
btVector3 _floorContact { 0.0f, 0.0f, 0.0f }; // internal, last floor contact point
btCollisionWorld* _world { nullptr }; // input, pointer to world
//btScalar _distanceToFeet { 0.0f }; // input, distance from object center to lowest point on shape
btScalar _halfHeight { 0.0f };
btScalar _radius { 0.0f };
btScalar _maxWallNormalUpComponent { 0.0f }; // input: max vertical component of wall normal
btScalar _maxStepHeight { 0.0f }; // input, max step height the character can climb
btConvexHullShape* _characterShape { nullptr }; // input, shape of character
CharacterGhostShape* _ghostShape { nullptr }; // internal, shape whose Aabb is used for overlap cache
int16_t _collisionFilterGroup { 0 };
int16_t _collisionFilterMask { 0 };
bool _inWorld { false }; // internal, was added to world
bool _hovering { false }; // internal,
bool _onFloor { false }; // output, is actually standing on floor
bool _steppingUp { false }; // output, future sweep hit a steppable ledge
bool _hasFloor { false }; // output, has floor underneath to fall on
bool _motorOnly { false }; // input, _linearVelocity slaves to _motorVelocity
};
#endif // hifi_CharacterGhostObject_h

31
libraries/physics/src/CharacterGhostShape.cpp Normal file
View file

@ -0,0 +1,31 @@
//
// CharacterGhostShape.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2016.09.14
// Copyright 2016 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 "CharacterGhostShape.h"
#include <assert.h>
CharacterGhostShape::CharacterGhostShape(const btConvexHullShape* shape) :
btConvexHullShape(reinterpret_cast<const btScalar*>(shape->getUnscaledPoints()), shape->getNumPoints(), sizeof(btVector3)) {
assert(shape);
assert(shape->getUnscaledPoints());
assert(shape->getNumPoints() > 0);
setMargin(shape->getMargin());
}
void CharacterGhostShape::getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const {
btConvexHullShape::getAabb(t, aabbMin, aabbMax);
// double the size of the Aabb by expanding both corners by half the extent
btVector3 expansion = 0.5f * (aabbMax - aabbMin);
aabbMin -= expansion;
aabbMax += expansion;
}

25
libraries/physics/src/CharacterGhostShape.h Normal file
View file

@ -0,0 +1,25 @@
//
// CharacterGhostShape.h
// libraries/physics/src
//
// Created by Andrew Meadows 2016.09.14
// Copyright 2016 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_CharacterGhostShape_h
#define hifi_CharacterGhostShape_h
#include <BulletCollision/CollisionShapes/btConvexHullShape.h>
class CharacterGhostShape : public btConvexHullShape {
// Same as btConvexHullShape but reports an expanded Aabb for larger ghost overlap cache
public:
CharacterGhostShape(const btConvexHullShape* shape);
virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const override;
};
#endif // hifi_CharacterGhostShape_h

31
libraries/physics/src/CharacterRayResult.cpp Executable file
View file

@ -0,0 +1,31 @@
//
// CharaterRayResult.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2016.09.05
// Copyright 2016 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 "CharacterRayResult.h"
#include <assert.h>
#include "CharacterGhostObject.h"
CharacterRayResult::CharacterRayResult (const CharacterGhostObject* character) :
btCollisionWorld::ClosestRayResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)),
_character(character)
{
assert(_character);
_character->getCollisionGroupAndMask(m_collisionFilterGroup, m_collisionFilterMask);
}
btScalar CharacterRayResult::addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) {
if (rayResult.m_collisionObject == _character) {
return 1.0f;
}
return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
}

44
libraries/physics/src/CharacterRayResult.h Normal file
View file

@ -0,0 +1,44 @@
//
// CharaterRayResult.h
// libraries/physics/src
//
// Created by Andrew Meadows 2016.09.05
// Copyright 2016 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_CharacterRayResult_h
#define hifi_CharacterRayResult_h
#include <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
class CharacterGhostObject;
class CharacterRayResult : public btCollisionWorld::ClosestRayResultCallback {
public:
CharacterRayResult (const CharacterGhostObject* character);
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) override;
protected:
const CharacterGhostObject* _character;
// Note: Public data members inherited from ClosestRayResultCallback
//
// btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction
// btVector3 m_rayToWorld;
// btVector3 m_hitNormalWorld;
// btVector3 m_hitPointWorld;
//
// Note: Public data members inherited from RayResultCallback
//
// btScalar m_closestHitFraction;
// const btCollisionObject* m_collisionObject;
// short int m_collisionFilterGroup;
// short int m_collisionFilterMask;
};
#endif // hifi_CharacterRayResult_h

42
libraries/physics/src/CharacterSweepResult.cpp Executable file
View file

@ -0,0 +1,42 @@
//
// CharaterSweepResult.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2016.09.01
// Copyright 2016 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 "CharacterSweepResult.h"
#include <assert.h>
#include "CharacterGhostObject.h"
CharacterSweepResult::CharacterSweepResult(const CharacterGhostObject* character)
: btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0f, 0.0f, 0.0f), btVector3(0.0f, 0.0f, 0.0f)),
_character(character)
{
// set collision group and mask to match _character
assert(_character);
_character->getCollisionGroupAndMask(m_collisionFilterGroup, m_collisionFilterMask);
}
btScalar CharacterSweepResult::addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool useWorldFrame) {
// skip objects that we shouldn't collide with
if (!convexResult.m_hitCollisionObject->hasContactResponse()) {
return btScalar(1.0);
}
if (convexResult.m_hitCollisionObject == _character) {
return btScalar(1.0);
}
return ClosestConvexResultCallback::addSingleResult(convexResult, useWorldFrame);
}
void CharacterSweepResult::resetHitHistory() {
m_hitCollisionObject = nullptr;
m_closestHitFraction = btScalar(1.0f);
}

45
libraries/physics/src/CharacterSweepResult.h Normal file
View file

@ -0,0 +1,45 @@
//
// CharaterSweepResult.h
// libraries/physics/src
//
// Created by Andrew Meadows 2016.09.01
// Copyright 2016 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_CharacterSweepResult_h
#define hifi_CharacterSweepResult_h
#include <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
class CharacterGhostObject;
class CharacterSweepResult : public btCollisionWorld::ClosestConvexResultCallback {
public:
CharacterSweepResult(const CharacterGhostObject* character);
virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool useWorldFrame) override;
void resetHitHistory();
protected:
const CharacterGhostObject* _character;
// NOTE: Public data members inherited from ClosestConvexResultCallback:
//
// btVector3 m_convexFromWorld; // unused except by btClosestNotMeConvexResultCallback
// btVector3 m_convexToWorld; // unused except by btClosestNotMeConvexResultCallback
// btVector3 m_hitNormalWorld;
// btVector3 m_hitPointWorld;
// const btCollisionObject* m_hitCollisionObject;
//
// NOTE: Public data members inherited from ConvexResultCallback:
//
// btScalar m_closestHitFraction;
// short int m_collisionFilterGroup;
// short int m_collisionFilterMask;
};
#endif // hifi_CharacterSweepResult_h

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

@ -1,6 +1,6 @@
//
// CollisionRenderMeshCache.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2016.07.13
// Copyright 2016 High Fidelity, Inc.

2
libraries/physics/src/CollisionRenderMeshCache.h
View file

@ -1,6 +1,6 @@
//
// CollisionRenderMeshCache.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2016.07.13
// Copyright 2016 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// ContactEvent.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2015.01.20
// Copyright 2015 High Fidelity, Inc.

2
libraries/physics/src/ContactInfo.h
View file

@ -1,6 +1,6 @@
//
// ContactEvent.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2015.01.20
// Copyright 2015 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// ObjectAction.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Seth Alves 2015-6-2
// Copyright 2015 High Fidelity, Inc.

2
libraries/physics/src/ObjectAction.h
View file

@ -1,6 +1,6 @@
//
// ObjectAction.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Seth Alves 2015-6-2
// Copyright 2015 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// ObjectMotionState.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.11.05
// Copyright 2014 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// ObjectMotionState.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.11.05
// Copyright 2014 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// PhysicalEntitySimulation.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2015.04.27
// Copyright 2015 High Fidelity, Inc.

2
libraries/physics/src/PhysicalEntitySimulation.h
View file

@ -1,6 +1,6 @@
//
// PhysicalEntitySimulation.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2015.04.27
// Copyright 2015 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// PhysicsEngine.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.10.29
// Copyright 2014 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// PhysicsEngine.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.10.29
// Copyright 2014 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// ShapeFactory.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.12.01
// Copyright 2014 High Fidelity, Inc.

2
libraries/physics/src/ShapeFactory.h
View file

@ -1,6 +1,6 @@
//
// ShapeFactory.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.12.01
// Copyright 2014 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// ShapeManager.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.10.29
// Copyright 2014 High Fidelity, Inc.

2
libraries/physics/src/ShapeManager.h
View file

@ -1,6 +1,6 @@
//
// ShapeManager.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.10.29
// Copyright 2014 High Fidelity, Inc.

2
libraries/shared/src/BackgroundMode.h
View file

@ -1,6 +1,6 @@
//
// BackgroundMode.h
// libraries/physcis/src
// libraries/physics/src
//
// Copyright 2015 High Fidelity, Inc.
//

39
libraries/shared/src/GeometryUtil.cpp
View file

@ -578,3 +578,42 @@ float coneSphereAngle(const glm::vec3& coneCenter, const glm::vec3& coneDirectio
return glm::max(0.0f, theta - phi);
}
glm::vec3 nearestPointOnLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd) {
glm::vec3 n = segmentEnd - segmentStart;
float len = glm::length(n);
if (len < EPSILON) {
return segmentStart;
} else {
n /= len;
float projection = glm::dot(point - segmentStart, n);
projection = glm::clamp(projection, 0.0f, len);
return segmentStart + n * projection;
}
}
float distanceFromLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd) {
return glm::length(point - nearestPointOnLineSegment(point, segmentStart, segmentEnd));
}
float distanceFromCapsule(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd, float capsuleRadius) {
return distanceFromLineSegment(point, segmentStart, segmentEnd) - capsuleRadius;
}
bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius) {
return distanceFromLineSegment(point, capsuleStart, capsuleEnd) < capsuleRadius;
}
glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius) {
glm::vec3 nearestPoint = nearestPointOnLineSegment(point, capsuleStart, capsuleEnd);
glm::vec3 d = point - nearestPoint;
float dLen = glm::length(d);
if (dLen < EPSILON) {
return nearestPoint; // TODO: maybe we should pick a point actually on the surface...
} else {
return nearestPoint + d * (capsuleRadius / dLen);
}
}

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

@ -160,5 +160,10 @@ private:
static void copyCleanArray(int& lengthA, glm::vec2* vertexArrayA, int& lengthB, glm::vec2* vertexArrayB);
};
glm::vec3 nearestPointOnLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd);
float distanceFromLineSegment(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd);
float distanceFromCapsule(const glm::vec3& point, const glm::vec3& segmentStart, const glm::vec3& segmentEnd, float capsuleRadius);
bool pointIsInsideCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius);
glm::vec3 projectPointOntoCapsule(const glm::vec3& point, const glm::vec3& capsuleStart, const glm::vec3& capsuleEnd, float capsuleRadius);
#endif // hifi_GeometryUtil_h

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

@ -1,6 +1,6 @@
//
// ShapeInfo.cpp
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.10.29
// Copyright 2014 High Fidelity, Inc.

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

@ -1,6 +1,6 @@
//
// ShapeInfo.h
// libraries/physcis/src
// libraries/physics/src
//
// Created by Andrew Meadows 2014.10.29
// Copyright 2014 High Fidelity, Inc.

78
scripts/system/controllers/teleport.js
View file

@ -13,7 +13,7 @@
var inTeleportMode = false;
var SMOOTH_ARRIVAL_SPACING = 33;
var NUMBER_OF_STEPS = 6;
var NUMBER_OF_STEPS_FOR_TELEPORT = 6;
var TARGET_MODEL_URL = Script.resolvePath("../assets/models/teleport-destination.fbx");
var TOO_CLOSE_MODEL_URL = Script.resolvePath("../assets/models/teleport-cancel.fbx");
@ -44,6 +44,7 @@ var COLORS_TELEPORT_TOO_CLOSE = {
var TELEPORT_CANCEL_RANGE = 1;
var USE_COOL_IN = true;
var COOL_IN_DURATION = 500;
var MAX_HMD_AVATAR_SEPARATION = 10.0;
function ThumbPad(hand) {
this.hand = hand;
@ -88,6 +89,7 @@ function Teleporter() {
this.updateConnected = null;
this.smoothArrivalInterval = null;
this.teleportHand = null;
this.teleportMode = "HMDAndAvatarTogether";
this.tooClose = false;
this.inCoolIn = false;
@ -543,43 +545,68 @@ function Teleporter() {
var offset = getAvatarFootOffset();
this.intersection.intersection.y += offset;
this.exitTeleportMode();
if (MyAvatar.hmdLeanRecenterEnabled) {
if (Vec3.distance(MyAvatar.position, _this.intersection.intersection) > MAX_HMD_AVATAR_SEPARATION) {
this.teleportMode = "HMDAndAvatarTogether";
} else {
this.teleportMode = "HMDFirstAvatarWillFollow";
}
} else {
this.teleportMode = "AvatarOnly";
}
// Disable smooth arrival, possibly temporarily
//this.smoothArrival();
MyAvatar.position = _this.intersection.intersection;
// Instead jump to the intersection directly.
var landingPoint = _this.intersection.intersection;
_this.teleportTo(landingPoint);
if (this.teleportMode === "HMDFirstAvatarWillFollow") {
MyAvatar.position = landingPoint;
}
// cleanup UI
_this.hideTargetOverlay();
_this.hideCancelOverlay();
HMD.centerUI();
}
};
this.findMidpoint = function(start, end) {
var xy = Vec3.sum(start, end);
var midpoint = Vec3.multiply(0.5, xy);
return midpoint
};
this.getWayPoints = function(startPoint, endPoint, numberOfSteps) {
var travel = Vec3.subtract(endPoint, startPoint);
var distance = Vec3.length(travel);
var wayPoints = [];
if (distance > 1.0) {
var base = Math.exp(Math.log(distance + 1.0) / numberOfSteps);
var i;
this.getArrivalPoints = function(startPoint, endPoint) {
var arrivalPoints = [];
var i;
var lastPoint;
for (i = 0; i < NUMBER_OF_STEPS; i++) {
if (i === 0) {
lastPoint = startPoint;
// this fancy math generates regular points in logarithmic space
for (i = 0; i < numberOfSteps - 1; i++) {
var backFraction = (1.0 - Math.exp((numberOfSteps - 1 - i) * Math.log(base))) / distance;
wayPoints.push(Vec3.sum(endPoint, Vec3.multiply(backFraction, travel)));
}
var newPoint = _this.findMidpoint(lastPoint, endPoint);
lastPoint = newPoint;
arrivalPoints.push(newPoint);
}
arrivalPoints.push(endPoint);
return arrivalPoints;
wayPoints.push(endPoint);
return wayPoints;
};
this.teleportTo = function(landingPoint) {
if (this.teleportMode === "AvatarOnly") {
MyAvatar.position = landingPoint;
} else if (this.teleportMode === "HMDAndAvatarTogether") {
var leanEnabled = MyAvatar.hmdLeanRecenterEnabled;
MyAvatar.setHMDLeanRecenterEnabled(false);
MyAvatar.position = landingPoint;
HMD.snapToAvatar();
MyAvatar.hmdLeanRecenterEnabled = leanEnabled;
} else if (this.teleportMode === "HMDFirstAvatarWillFollow") {
var eyeOffset = Vec3.subtract(MyAvatar.getEyePosition(), MyAvatar.position);
landingPoint = Vec3.sum(landingPoint, eyeOffset);
HMD.position = landingPoint;
}
}
this.smoothArrival = function() {
_this.arrivalPoints = _this.getArrivalPoints(MyAvatar.position, _this.intersection.intersection);
_this.arrivalPoints = _this.getWayPoints(MyAvatar.position, _this.intersection.intersection, NUMBER_OF_STEPS_FOR_TELEPORT);
_this.smoothArrivalInterval = Script.setInterval(function() {
if (_this.arrivalPoints.length === 0) {
Script.clearInterval(_this.smoothArrivalInterval);
@ -587,7 +614,7 @@ function Teleporter() {
return;
}
var landingPoint = _this.arrivalPoints.shift();
MyAvatar.position = landingPoint;
_this.teleportTo(landingPoint);
if (_this.arrivalPoints.length === 1 || _this.arrivalPoints.length === 0) {
_this.hideTargetOverlay();
@ -599,7 +626,6 @@ function Teleporter() {
this.createTargetOverlay(false);
this.createCancelOverlay(false);
}
//related to repositioning the avatar after you teleport