diff --git a/examples/swissArmyJetpack.js b/examples/swissArmyJetpack.js index fb4dc1dc04..4cb68e9b64 100644 --- a/examples/swissArmyJetpack.js +++ b/examples/swissArmyJetpack.js @@ -178,6 +178,10 @@ function disableArtificialGravity() { MyAvatar.motionBehaviors = MyAvatar.motionBehaviors & ~AVATAR_MOTION_OBEY_LOCAL_GRAVITY; updateButton(3, false); } +// call this immediately so that avatar doesn't fall before voxel data arrives +// Ideally we would only do this on LOGIN, not when starting the script +// in the middle of a session. +disableArtificialGravity(); function enableArtificialGravity() { // NOTE: setting the gravity automatically sets the AVATAR_MOTION_OBEY_LOCAL_GRAVITY behavior bit. @@ -276,7 +280,6 @@ function update(deltaTime) { } Script.update.connect(update); - // we also handle click detection in our mousePressEvent() function mousePressEvent(event) { var clickedOverlay = Overlays.getOverlayAtPoint({x: event.x, y: event.y}); diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 7074f620bf..4af38e8770 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -190,7 +190,7 @@ Menu::Menu() : addDisabledActionAndSeparator(editMenu, "Physics"); QObject* avatar = appInstance->getAvatar(); addCheckableActionToQMenuAndActionHash(editMenu, MenuOption::ObeyEnvironmentalGravity, Qt::SHIFT | Qt::Key_G, true, - avatar, SLOT(updateMotionBehaviors())); + avatar, SLOT(updateMotionBehaviorsFromMenu())); addAvatarCollisionSubMenu(editMenu); diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp index 1ee22d3edf..c925e452b2 100644 --- a/interface/src/avatar/Hand.cpp +++ b/interface/src/avatar/Hand.cpp @@ -13,7 +13,6 @@ #include #include -#include #include "Application.h" #include "Avatar.h" diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 652eb56258..cd799b7d2e 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -45,7 +45,13 @@ const float PITCH_SPEED = 100.0f; // degrees/sec const float COLLISION_RADIUS_SCALAR = 1.2f; // pertains to avatar-to-avatar collisions const float COLLISION_RADIUS_SCALE = 0.125f; -const float DATA_SERVER_LOCATION_CHANGE_UPDATE_MSECS = 5 * 1000; +const float DATA_SERVER_LOCATION_CHANGE_UPDATE_MSECS = 5.0f * 1000.0f; + +// TODO: normalize avatar speed for standard avatar size, then scale all motion logic +// to properly follow avatar size. +float DEFAULT_MOTOR_TIMESCALE = 0.25f; +float MAX_AVATAR_SPEED = 300.0f; +float MAX_MOTOR_SPEED = MAX_AVATAR_SPEED; MyAvatar::MyAvatar() : Avatar(), @@ -55,13 +61,15 @@ MyAvatar::MyAvatar() : _shouldJump(false), _gravity(0.0f, -1.0f, 0.0f), _distanceToNearestAvatar(std::numeric_limits::max()), - _lastCollisionPosition(0, 0, 0), - _speedBrakes(false), + _wasPushing(false), + _isPushing(false), _thrust(0.0f), - _isThrustOn(false), - _thrustMultiplier(1.0f), - _motionBehaviors(0), + _motorVelocity(0.0f), + _motorTimescale(DEFAULT_MOTOR_TIMESCALE), + _maxMotorSpeed(MAX_MOTOR_SPEED), + _motionBehaviors(AVATAR_MOTION_DEFAULTS), _lastBodyPenetration(0.0f), + _lastFloorContactPoint(0.0f), _lookAtTargetAvatar(), _shouldRender(true), _billboardValid(false), @@ -115,130 +123,56 @@ void MyAvatar::update(float deltaTime) { void MyAvatar::simulate(float deltaTime) { - glm::quat orientation = getOrientation(); - if (_scale != _targetScale) { float scale = (1.0f - SMOOTHING_RATIO) * _scale + SMOOTHING_RATIO * _targetScale; setScale(scale); Application::getInstance()->getCamera()->setScale(scale); } - // Collect thrust forces from keyboard and devices - updateThrust(deltaTime); - // update the movement of the hand and process handshaking with other avatars... updateHandMovementAndTouching(deltaTime); - // apply gravity - // For gravity, always move the avatar by the amount driven by gravity, so that the collision - // routines will detect it and collide every frame when pulled by gravity to a surface - const float MIN_DISTANCE_AFTER_COLLISION_FOR_GRAVITY = 0.02f; - if (glm::length(_position - _lastCollisionPosition) > MIN_DISTANCE_AFTER_COLLISION_FOR_GRAVITY) { + updateOrientation(deltaTime); + + float keyboardInput = fabsf(_driveKeys[FWD] - _driveKeys[BACK]) + + fabsf(_driveKeys[RIGHT] - _driveKeys[LEFT]) + + fabsf(_driveKeys[UP] - _driveKeys[DOWN]); + + bool walkingOnFloor = false; + float gravityLength = glm::length(_gravity); + if (gravityLength > EPSILON) { + const CapsuleShape& boundingShape = _skeletonModel.getBoundingShape(); + glm::vec3 startCap; + boundingShape.getStartPoint(startCap); + glm::vec3 bottomOfBoundingCapsule = startCap + (boundingShape.getRadius() / gravityLength) * _gravity; + + float fallThreshold = 2.f * deltaTime * gravityLength; + walkingOnFloor = (glm::distance(bottomOfBoundingCapsule, _lastFloorContactPoint) < fallThreshold); + } + + if (keyboardInput > 0.0f || glm::length2(_velocity) > 0.0f || glm::length2(_thrust) > 0.0f || + ! walkingOnFloor) { + // apply gravity _velocity += _scale * _gravity * (GRAVITY_EARTH * deltaTime); - } + + // update motor and thrust + updateMotorFromKeyboard(deltaTime, walkingOnFloor); + applyMotor(deltaTime); + applyThrust(deltaTime); - // add thrust to velocity - _velocity += _thrust * deltaTime; - - // update body yaw by body yaw delta - orientation = orientation * glm::quat(glm::radians( - glm::vec3(_bodyPitchDelta, _bodyYawDelta, _bodyRollDelta) * deltaTime)); - // decay body rotation momentum - - const float BODY_SPIN_FRICTION = 7.5f; - float bodySpinMomentum = 1.0f - BODY_SPIN_FRICTION * deltaTime; - if (bodySpinMomentum < 0.0f) { bodySpinMomentum = 0.0f; } - _bodyPitchDelta *= bodySpinMomentum; - _bodyYawDelta *= bodySpinMomentum; - _bodyRollDelta *= bodySpinMomentum; - - float MINIMUM_ROTATION_RATE = 2.0f; - if (fabs(_bodyYawDelta) < MINIMUM_ROTATION_RATE) { _bodyYawDelta = 0.0f; } - if (fabs(_bodyRollDelta) < MINIMUM_ROTATION_RATE) { _bodyRollDelta = 0.0f; } - if (fabs(_bodyPitchDelta) < MINIMUM_ROTATION_RATE) { _bodyPitchDelta = 0.0f; } - - const float MAX_STATIC_FRICTION_SPEED = 0.5f; - const float STATIC_FRICTION_STRENGTH = _scale * 20.0f; - applyStaticFriction(deltaTime, _velocity, MAX_STATIC_FRICTION_SPEED, STATIC_FRICTION_STRENGTH); - - // Damp avatar velocity - const float LINEAR_DAMPING_STRENGTH = 0.5f; - const float SPEED_BRAKE_POWER = _scale * 10.0f; - const float SQUARED_DAMPING_STRENGTH = 0.007f; - - const float SLOW_NEAR_RADIUS = 5.0f; - float linearDamping = LINEAR_DAMPING_STRENGTH; - const float NEAR_AVATAR_DAMPING_FACTOR = 50.0f; - if (_distanceToNearestAvatar < _scale * SLOW_NEAR_RADIUS) { - linearDamping *= 1.0f + NEAR_AVATAR_DAMPING_FACTOR * - ((SLOW_NEAR_RADIUS - _distanceToNearestAvatar) / SLOW_NEAR_RADIUS); - } - if (_speedBrakes) { - applyDamping(deltaTime, _velocity, linearDamping * SPEED_BRAKE_POWER, SQUARED_DAMPING_STRENGTH * SPEED_BRAKE_POWER); - } else { - applyDamping(deltaTime, _velocity, linearDamping, SQUARED_DAMPING_STRENGTH); - } - - if (OculusManager::isConnected()) { - // these angles will be in radians - float yaw, pitch, roll; - OculusManager::getEulerAngles(yaw, pitch, roll); - // ... so they need to be converted to degrees before we do math... - - // The neck is limited in how much it can yaw, so we check its relative - // yaw from the body and yaw the body if necessary. - yaw *= DEGREES_PER_RADIAN; - float bodyToHeadYaw = yaw - _oculusYawOffset; - const float MAX_NECK_YAW = 85.0f; // degrees - if ((fabs(bodyToHeadYaw) > 2.0f * MAX_NECK_YAW) && (yaw * _oculusYawOffset < 0.0f)) { - // We've wrapped around the range for yaw so adjust - // the measured yaw to be relative to _oculusYawOffset. - if (yaw > 0.0f) { - yaw -= 360.0f; - } else { - yaw += 360.0f; - } - bodyToHeadYaw = yaw - _oculusYawOffset; + // update position + if (glm::length2(_velocity) < EPSILON) { + _velocity = glm::vec3(0.0f); + } else { + _position += _velocity * deltaTime; } - - float delta = fabs(bodyToHeadYaw) - MAX_NECK_YAW; - if (delta > 0.0f) { - yaw = MAX_NECK_YAW; - if (bodyToHeadYaw < 0.0f) { - delta *= -1.0f; - bodyToHeadYaw = -MAX_NECK_YAW; - } else { - bodyToHeadYaw = MAX_NECK_YAW; - } - // constrain _oculusYawOffset to be within range [-180,180] - _oculusYawOffset = fmod((_oculusYawOffset + delta) + 180.0f, 360.0f) - 180.0f; - - // We must adjust the body orientation using a delta rotation (rather than - // doing yaw math) because the body's yaw ranges are not the same - // as what the Oculus API provides. - glm::vec3 UP_AXIS = glm::vec3(0.0f, 1.0f, 0.0f); - glm::quat bodyCorrection = glm::angleAxis(glm::radians(delta), UP_AXIS); - orientation = orientation * bodyCorrection; - } - Head* head = getHead(); - head->setBaseYaw(bodyToHeadYaw); - - head->setBasePitch(pitch * DEGREES_PER_RADIAN); - head->setBaseRoll(roll * DEGREES_PER_RADIAN); } - // update the euler angles - setOrientation(orientation); - // update moving flag based on speed const float MOVING_SPEED_THRESHOLD = 0.01f; - float speed = glm::length(_velocity); - _moving = speed > MOVING_SPEED_THRESHOLD; - + _moving = glm::length(_velocity) > MOVING_SPEED_THRESHOLD; updateChatCircle(deltaTime); - _position += _velocity * deltaTime; - // update avatar skeleton and simulate hand and head getHand()->collideAgainstOurself(); getHand()->simulate(deltaTime, true); @@ -261,9 +195,6 @@ void MyAvatar::simulate(float deltaTime) { head->setScale(_scale); head->simulate(deltaTime, true); - // Zero thrust out now that we've added it to velocity in this frame - _thrust *= glm::vec3(0.0f); - // now that we're done stepping the avatar forward in time, compute new collisions if (_collisionGroups != 0) { Camera* myCamera = Application::getInstance()->getCamera(); @@ -633,6 +564,214 @@ bool MyAvatar::shouldRenderHead(const glm::vec3& cameraPosition, RenderMode rend (glm::length(cameraPosition - head->calculateAverageEyePosition()) > RENDER_HEAD_CUTOFF_DISTANCE * _scale); } +void MyAvatar::updateOrientation(float deltaTime) { + // Gather rotation information from keyboard + _bodyYawDelta -= _driveKeys[ROT_RIGHT] * YAW_SPEED * deltaTime; + _bodyYawDelta += _driveKeys[ROT_LEFT] * YAW_SPEED * deltaTime; + getHead()->setBasePitch(getHead()->getBasePitch() + (_driveKeys[ROT_UP] - _driveKeys[ROT_DOWN]) * PITCH_SPEED * deltaTime); + + // update body yaw by body yaw delta + glm::quat orientation = getOrientation() * glm::quat(glm::radians( + glm::vec3(_bodyPitchDelta, _bodyYawDelta, _bodyRollDelta) * deltaTime)); + + // decay body rotation momentum + const float BODY_SPIN_FRICTION = 7.5f; + float bodySpinMomentum = 1.0f - BODY_SPIN_FRICTION * deltaTime; + if (bodySpinMomentum < 0.0f) { bodySpinMomentum = 0.0f; } + _bodyPitchDelta *= bodySpinMomentum; + _bodyYawDelta *= bodySpinMomentum; + _bodyRollDelta *= bodySpinMomentum; + + float MINIMUM_ROTATION_RATE = 2.0f; + if (fabs(_bodyYawDelta) < MINIMUM_ROTATION_RATE) { _bodyYawDelta = 0.0f; } + if (fabs(_bodyRollDelta) < MINIMUM_ROTATION_RATE) { _bodyRollDelta = 0.0f; } + if (fabs(_bodyPitchDelta) < MINIMUM_ROTATION_RATE) { _bodyPitchDelta = 0.0f; } + + if (OculusManager::isConnected()) { + // these angles will be in radians + float yaw, pitch, roll; + OculusManager::getEulerAngles(yaw, pitch, roll); + // ... so they need to be converted to degrees before we do math... + + // The neck is limited in how much it can yaw, so we check its relative + // yaw from the body and yaw the body if necessary. + yaw *= DEGREES_PER_RADIAN; + float bodyToHeadYaw = yaw - _oculusYawOffset; + const float MAX_NECK_YAW = 85.0f; // degrees + if ((fabs(bodyToHeadYaw) > 2.0f * MAX_NECK_YAW) && (yaw * _oculusYawOffset < 0.0f)) { + // We've wrapped around the range for yaw so adjust + // the measured yaw to be relative to _oculusYawOffset. + if (yaw > 0.0f) { + yaw -= 360.0f; + } else { + yaw += 360.0f; + } + bodyToHeadYaw = yaw - _oculusYawOffset; + } + + float delta = fabs(bodyToHeadYaw) - MAX_NECK_YAW; + if (delta > 0.0f) { + yaw = MAX_NECK_YAW; + if (bodyToHeadYaw < 0.0f) { + delta *= -1.0f; + bodyToHeadYaw = -MAX_NECK_YAW; + } else { + bodyToHeadYaw = MAX_NECK_YAW; + } + // constrain _oculusYawOffset to be within range [-180,180] + _oculusYawOffset = fmod((_oculusYawOffset + delta) + 180.0f, 360.0f) - 180.0f; + + // We must adjust the body orientation using a delta rotation (rather than + // doing yaw math) because the body's yaw ranges are not the same + // as what the Oculus API provides. + glm::vec3 UP_AXIS = glm::vec3(0.0f, 1.0f, 0.0f); + glm::quat bodyCorrection = glm::angleAxis(glm::radians(delta), UP_AXIS); + orientation = orientation * bodyCorrection; + } + Head* head = getHead(); + head->setBaseYaw(bodyToHeadYaw); + + head->setBasePitch(pitch * DEGREES_PER_RADIAN); + head->setBaseRoll(roll * DEGREES_PER_RADIAN); + } + + // update the euler angles + setOrientation(orientation); +} + +void MyAvatar::updateMotorFromKeyboard(float deltaTime, bool walking) { + // Increase motor velocity until its length is equal to _maxMotorSpeed. + if (!(_motionBehaviors & AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED)) { + // nothing to do + return; + } + + glm::vec3 localVelocity = _velocity; + if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) { + glm::quat orientation = getHead()->getCameraOrientation(); + localVelocity = glm::inverse(orientation) * _velocity; + } + + // Compute keyboard input + glm::vec3 front = (_driveKeys[FWD] - _driveKeys[BACK]) * IDENTITY_FRONT; + glm::vec3 right = (_driveKeys[RIGHT] - _driveKeys[LEFT]) * IDENTITY_RIGHT; + glm::vec3 up = (_driveKeys[UP] - _driveKeys[DOWN]) * IDENTITY_UP; + + glm::vec3 direction = front + right + up; + float directionLength = glm::length(direction); + + // Compute motor magnitude + if (directionLength > EPSILON) { + direction /= directionLength; + // the finalMotorSpeed depends on whether we are walking or not + const float MIN_KEYBOARD_CONTROL_SPEED = 2.0f; + const float MAX_WALKING_SPEED = 3.0f * MIN_KEYBOARD_CONTROL_SPEED; + float finalMaxMotorSpeed = walking ? MAX_WALKING_SPEED : _maxMotorSpeed; + + float motorLength = glm::length(_motorVelocity); + if (motorLength < MIN_KEYBOARD_CONTROL_SPEED) { + // an active keyboard motor should never be slower than this + _motorVelocity = MIN_KEYBOARD_CONTROL_SPEED * direction; + } else { + float MOTOR_LENGTH_TIMESCALE = 1.5f; + float tau = glm::clamp(deltaTime / MOTOR_LENGTH_TIMESCALE, 0.0f, 1.0f); + float INCREASE_FACTOR = 2.0f; + //_motorVelocity *= 1.0f + tau * INCREASE_FACTOR; + motorLength *= 1.0f + tau * INCREASE_FACTOR; + if (motorLength > finalMaxMotorSpeed) { + motorLength = finalMaxMotorSpeed; + } + _motorVelocity = motorLength * direction; + } + _isPushing = true; + } else { + // motor opposes motion (wants to be at rest) + _motorVelocity = - localVelocity; + } +} + +float MyAvatar::computeMotorTimescale() { + // The timescale of the motor is the approximate time it takes for the motor to + // accomplish its intended velocity. A short timescale makes the motor strong, + // and a long timescale makes it weak. The value of timescale to use depends + // on what the motor is doing: + // + // (1) braking --> short timescale (aggressive motor assertion) + // (2) pushing --> medium timescale (mild motor assertion) + // (3) inactive --> long timescale (gentle friction for low speeds) + // + // TODO: recover extra braking behavior when flying close to nearest avatar + + float MIN_MOTOR_TIMESCALE = 0.125f; + float MAX_MOTOR_TIMESCALE = 0.5f; + float MIN_BRAKE_SPEED = 0.4f; + + float timescale = MAX_MOTOR_TIMESCALE; + float speed = glm::length(_velocity); + bool areThrusting = (glm::length2(_thrust) > EPSILON); + + if (_wasPushing && !(_isPushing || areThrusting) && speed > MIN_BRAKE_SPEED) { + // we don't change _wasPushing for this case --> + // keeps the brakes on until we go below MIN_BRAKE_SPEED + timescale = MIN_MOTOR_TIMESCALE; + } else { + if (_isPushing) { + timescale = _motorTimescale; + } + _wasPushing = _isPushing || areThrusting; + } + _isPushing = false; + return timescale; +} + +void MyAvatar::applyMotor(float deltaTime) { + if (!( _motionBehaviors & AVATAR_MOTION_MOTOR_ENABLED)) { + // nothing to do --> early exit + return; + } + glm::vec3 targetVelocity = _motorVelocity; + if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) { + // rotate _motorVelocity into world frame + glm::quat rotation = getOrientation(); + targetVelocity = rotation * _motorVelocity; + } + + glm::vec3 targetDirection(0.f); + if (glm::length2(targetVelocity) > EPSILON) { + targetDirection = glm::normalize(targetVelocity); + } + glm::vec3 deltaVelocity = targetVelocity - _velocity; + + if (_motionBehaviors & AVATAR_MOTION_MOTOR_COLLISION_SURFACE_ONLY && glm::length2(_gravity) > EPSILON) { + // For now we subtract the component parallel to gravity but what we need to do is: + // TODO: subtract the component perp to the local surface normal (motor only pushes in surface plane). + glm::vec3 gravityDirection = glm::normalize(_gravity); + glm::vec3 parallelDelta = glm::dot(deltaVelocity, gravityDirection) * gravityDirection; + if (glm::dot(targetVelocity, _velocity) > 0.0f) { + // remove parallel part from deltaVelocity + deltaVelocity -= parallelDelta; + } + } + + // simple critical damping + float timescale = computeMotorTimescale(); + float tau = glm::clamp(deltaTime / timescale, 0.0f, 1.0f); + _velocity += tau * deltaVelocity; +} + +void MyAvatar::applyThrust(float deltaTime) { + _velocity += _thrust * deltaTime; + float speed = glm::length(_velocity); + // cap the speed that thrust can achieve + if (speed > MAX_AVATAR_SPEED) { + _velocity *= MAX_AVATAR_SPEED / speed; + } + // zero thrust so we don't pile up thrust from other sources + _thrust = glm::vec3(0.0f); +} + +/* Keep this code for the short term as reference in case we need to further tune the new model + * to achieve legacy movement response. void MyAvatar::updateThrust(float deltaTime) { // // Gather thrust information from keyboard and sensors to apply to avatar motion @@ -678,10 +817,6 @@ void MyAvatar::updateThrust(float deltaTime) { } _lastBodyPenetration = glm::vec3(0.0f); - _bodyYawDelta -= _driveKeys[ROT_RIGHT] * YAW_SPEED * deltaTime; - _bodyYawDelta += _driveKeys[ROT_LEFT] * YAW_SPEED * deltaTime; - getHead()->setBasePitch(getHead()->getBasePitch() + (_driveKeys[ROT_UP] - _driveKeys[ROT_DOWN]) * PITCH_SPEED * deltaTime); - // If thrust keys are being held down, slowly increase thrust to allow reaching great speeds if (_driveKeys[FWD] || _driveKeys[BACK] || _driveKeys[RIGHT] || _driveKeys[LEFT] || _driveKeys[UP] || _driveKeys[DOWN]) { const float THRUST_INCREASE_RATE = 1.05f; @@ -712,8 +847,34 @@ void MyAvatar::updateThrust(float deltaTime) { if (_isThrustOn || (_speedBrakes && (glm::length(_velocity) < MIN_SPEED_BRAKE_VELOCITY))) { _speedBrakes = false; } + _velocity += _thrust * deltaTime; + // Zero thrust out now that we've added it to velocity in this frame + _thrust = glm::vec3(0.0f); + + // apply linear damping + const float MAX_STATIC_FRICTION_SPEED = 0.5f; + const float STATIC_FRICTION_STRENGTH = _scale * 20.0f; + applyStaticFriction(deltaTime, _velocity, MAX_STATIC_FRICTION_SPEED, STATIC_FRICTION_STRENGTH); + + const float LINEAR_DAMPING_STRENGTH = 0.5f; + const float SPEED_BRAKE_POWER = _scale * 10.0f; + const float SQUARED_DAMPING_STRENGTH = 0.007f; + + const float SLOW_NEAR_RADIUS = 5.0f; + float linearDamping = LINEAR_DAMPING_STRENGTH; + const float NEAR_AVATAR_DAMPING_FACTOR = 50.0f; + if (_distanceToNearestAvatar < _scale * SLOW_NEAR_RADIUS) { + linearDamping *= 1.0f + NEAR_AVATAR_DAMPING_FACTOR * + ((SLOW_NEAR_RADIUS - _distanceToNearestAvatar) / SLOW_NEAR_RADIUS); + } + if (_speedBrakes) { + applyDamping(deltaTime, _velocity, linearDamping * SPEED_BRAKE_POWER, SQUARED_DAMPING_STRENGTH * SPEED_BRAKE_POWER); + } else { + applyDamping(deltaTime, _velocity, linearDamping, SQUARED_DAMPING_STRENGTH); + } } +*/ void MyAvatar::updateHandMovementAndTouching(float deltaTime) { glm::quat orientation = getOrientation(); @@ -760,7 +921,6 @@ void MyAvatar::updateCollisionWithEnvironment(float deltaTime, float radius) { if (Application::getInstance()->getEnvironment()->findCapsulePenetration( _position - up * (pelvisFloatingHeight - radius), _position + up * (getSkeletonHeight() - pelvisFloatingHeight + radius), radius, penetration)) { - _lastCollisionPosition = _position; updateCollisionSound(penetration, deltaTime, ENVIRONMENT_COLLISION_FREQUENCY); applyHardCollision(penetration, ENVIRONMENT_SURFACE_ELASTICITY, ENVIRONMENT_SURFACE_DAMPING); } @@ -772,12 +932,59 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) { myCollisions.clear(); const CapsuleShape& boundingShape = _skeletonModel.getBoundingShape(); if (Application::getInstance()->getVoxelTree()->findShapeCollisions(&boundingShape, myCollisions)) { - const float VOXEL_ELASTICITY = 0.4f; + const float VOXEL_ELASTICITY = 0.0f; const float VOXEL_DAMPING = 0.0f; - for (int i = 0; i < myCollisions.size(); ++i) { - CollisionInfo* collision = myCollisions[i]; - applyHardCollision(collision->_penetration, VOXEL_ELASTICITY, VOXEL_DAMPING); + + if (glm::length2(_gravity) > EPSILON) { + if (myCollisions.size() == 1) { + // trivial case + CollisionInfo* collision = myCollisions[0]; + applyHardCollision(collision->_penetration, VOXEL_ELASTICITY, VOXEL_DAMPING); + _lastFloorContactPoint = collision->_contactPoint - collision->_penetration; + } else { + // This is special collision handling for when walking on a voxel field which + // prevents snagging at corners and seams. + + // sift through the collisions looking for one against the "floor" + int floorIndex = 0; + float distanceToFloor = 0.0f; + float penetrationWithFloor = 0.0f; + for (int i = 0; i < myCollisions.size(); ++i) { + CollisionInfo* collision = myCollisions[i]; + float distance = glm::dot(_gravity, collision->_contactPoint - _position); + if (distance > distanceToFloor) { + distanceToFloor = distance; + penetrationWithFloor = glm::dot(_gravity, collision->_penetration); + floorIndex = i; + } + } + + // step through the collisions again and apply each that is not redundant + glm::vec3 oldPosition = _position; + for (int i = 0; i < myCollisions.size(); ++i) { + CollisionInfo* collision = myCollisions[i]; + if (i == floorIndex) { + applyHardCollision(collision->_penetration, VOXEL_ELASTICITY, VOXEL_DAMPING); + _lastFloorContactPoint = collision->_contactPoint - collision->_penetration; + } else { + float distance = glm::dot(_gravity, collision->_contactPoint - oldPosition); + float penetration = glm::dot(_gravity, collision->_penetration); + if (fabsf(distance - distanceToFloor) > penetrationWithFloor || penetration > penetrationWithFloor) { + // resolution of the deepest penetration would not resolve this one + // so we apply the collision + applyHardCollision(collision->_penetration, VOXEL_ELASTICITY, VOXEL_DAMPING); + } + } + } + } + } else { + // no gravity -- apply all collisions + for (int i = 0; i < myCollisions.size(); ++i) { + CollisionInfo* collision = myCollisions[i]; + applyHardCollision(collision->_penetration, VOXEL_ELASTICITY, VOXEL_DAMPING); + } } + const float VOXEL_COLLISION_FREQUENCY = 0.5f; updateCollisionSound(myCollisions[0]->_penetration, deltaTime, VOXEL_COLLISION_FREQUENCY); } @@ -1141,8 +1348,7 @@ void MyAvatar::goToLocationFromResponse(const QJsonObject& jsonObject) { } } -void MyAvatar::updateMotionBehaviors() { - _motionBehaviors = 0; +void MyAvatar::updateMotionBehaviorsFromMenu() { if (Menu::getInstance()->isOptionChecked(MenuOption::ObeyEnvironmentalGravity)) { _motionBehaviors |= AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY; // Environmental and Local gravities are incompatible. Environmental setting trumps local. @@ -1162,8 +1368,14 @@ void MyAvatar::setCollisionGroups(quint32 collisionGroups) { menu->setIsOptionChecked(MenuOption::CollideWithParticles, (bool)(_collisionGroups & COLLISION_GROUP_PARTICLES)); } -void MyAvatar::setMotionBehaviors(quint32 flags) { - _motionBehaviors = flags; +void MyAvatar::setMotionBehaviorsByScript(quint32 flags) { + // start with the defaults + _motionBehaviors = AVATAR_MOTION_DEFAULTS; + + // add the set scriptable bits + _motionBehaviors += flags & AVATAR_MOTION_SCRIPTABLE_BITS; + + // reconcile incompatible settings from menu (if any) Menu* menu = Menu::getInstance(); menu->setIsOptionChecked(MenuOption::ObeyEnvironmentalGravity, (bool)(_motionBehaviors & AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY)); // Environmental and Local gravities are incompatible. Environmental setting trumps local. diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index bbf3d05189..0c02e42eb0 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -28,7 +28,7 @@ enum AvatarHandState class MyAvatar : public Avatar { Q_OBJECT Q_PROPERTY(bool shouldRenderLocally READ getShouldRenderLocally WRITE setShouldRenderLocally) - Q_PROPERTY(quint32 motionBehaviors READ getMotionBehaviors WRITE setMotionBehaviors) + Q_PROPERTY(quint32 motionBehaviors READ getMotionBehaviorsForScript WRITE setMotionBehaviorsByScript) Q_PROPERTY(glm::vec3 gravity READ getGravity WRITE setLocalGravity) public: @@ -90,8 +90,9 @@ public: virtual void setSkeletonModelURL(const QUrl& skeletonModelURL); virtual void setCollisionGroups(quint32 collisionGroups); - void setMotionBehaviors(quint32 flags); - quint32 getMotionBehaviors() const { return _motionBehaviors; } + + void setMotionBehaviorsByScript(quint32 flags); + quint32 getMotionBehaviorsForScript() const { return _motionBehaviors & AVATAR_MOTION_SCRIPTABLE_BITS; } void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration); @@ -109,7 +110,7 @@ public slots: glm::vec3 getThrust() { return _thrust; }; void setThrust(glm::vec3 newThrust) { _thrust = newThrust; } - void updateMotionBehaviors(); + void updateMotionBehaviorsFromMenu(); signals: void transformChanged(); @@ -123,17 +124,18 @@ private: glm::vec3 _gravity; glm::vec3 _environmentGravity; float _distanceToNearestAvatar; // How close is the nearest avatar? - - // motion stuff - glm::vec3 _lastCollisionPosition; - bool _speedBrakes; - glm::vec3 _thrust; // final acceleration for the current frame - bool _isThrustOn; - float _thrustMultiplier; + bool _wasPushing; + bool _isPushing; + glm::vec3 _thrust; // final acceleration from outside sources for the current frame + + glm::vec3 _motorVelocity; // intended velocity of avatar motion + float _motorTimescale; // timescale for avatar motor to achieve its desired velocity + float _maxMotorSpeed; quint32 _motionBehaviors; glm::vec3 _lastBodyPenetration; + glm::vec3 _lastFloorContactPoint; QWeakPointer _lookAtTargetAvatar; glm::vec3 _targetAvatarPosition; bool _shouldRender; @@ -141,7 +143,11 @@ private: float _oculusYawOffset; // private methods - void updateThrust(float deltaTime); + void updateOrientation(float deltaTime); + void updateMotorFromKeyboard(float deltaTime, bool walking); + float computeMotorTimescale(); + void applyMotor(float deltaTime); + void applyThrust(float deltaTime); void updateHandMovementAndTouching(float deltaTime); void updateCollisionWithAvatars(float deltaTime); void updateCollisionWithEnvironment(float deltaTime, float radius); diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index a177783955..7141ccb32f 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -592,18 +592,10 @@ void Model::rebuildShapes() { capsule->setRotation(combinedRotations[i] * joint.shapeRotation); _jointShapes.push_back(capsule); - glm::vec3 endPoint; - capsule->getEndPoint(endPoint); - glm::vec3 startPoint; - capsule->getStartPoint(startPoint); - - // add some points that bound a sphere at the center of the capsule - glm::vec3 axis = glm::vec3(radius); - shapeExtents.addPoint(worldPosition + axis); - shapeExtents.addPoint(worldPosition - axis); - // add the two furthest surface points of the capsule - axis = (halfHeight + radius) * glm::normalize(endPoint - startPoint); + glm::vec3 axis; + capsule->computeNormalizedAxis(axis); + axis = halfHeight * axis + glm::vec3(radius); shapeExtents.addPoint(worldPosition + axis); shapeExtents.addPoint(worldPosition - axis); @@ -637,7 +629,7 @@ void Model::rebuildShapes() { glm::quat inverseRotation = glm::inverse(_rotation); glm::vec3 rootPosition = extractTranslation(transforms[rootIndex]); _boundingShapeLocalOffset = inverseRotation * (0.5f * (totalExtents.maximum + totalExtents.minimum) - rootPosition); - _boundingShape.setPosition(_translation - _rotation * _boundingShapeLocalOffset); + _boundingShape.setPosition(_translation + _rotation * _boundingShapeLocalOffset); _boundingShape.setRotation(_rotation); } diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h index be47aed1ba..af0b0c57d6 100755 --- a/libraries/avatars/src/AvatarData.h +++ b/libraries/avatars/src/AvatarData.h @@ -51,8 +51,24 @@ typedef unsigned long long quint64; #include "HandData.h" // avatar motion behaviors -const quint32 AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY = 1U << 0; -const quint32 AVATAR_MOTION_OBEY_LOCAL_GRAVITY = 1U << 1; +const quint32 AVATAR_MOTION_MOTOR_ENABLED = 1U << 0; +const quint32 AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED = 1U << 1; +const quint32 AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME = 1U << 2; +const quint32 AVATAR_MOTION_MOTOR_COLLISION_SURFACE_ONLY = 1U << 3; + +const quint32 AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY = 1U << 4; +const quint32 AVATAR_MOTION_OBEY_LOCAL_GRAVITY = 1U << 5; + +const quint32 AVATAR_MOTION_DEFAULTS = + AVATAR_MOTION_MOTOR_ENABLED | + AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED | + AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME; + +// these bits will be expanded as features are exposed +const quint32 AVATAR_MOTION_SCRIPTABLE_BITS = + AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY | + AVATAR_MOTION_OBEY_LOCAL_GRAVITY; + // First bitset const int KEY_STATE_START_BIT = 0; // 1st and 2nd bits diff --git a/libraries/shared/src/CapsuleShape.cpp b/libraries/shared/src/CapsuleShape.cpp index 5055b3636e..11bd70f8d2 100644 --- a/libraries/shared/src/CapsuleShape.cpp +++ b/libraries/shared/src/CapsuleShape.cpp @@ -34,6 +34,7 @@ CapsuleShape::CapsuleShape(float radius, float halfHeight, const glm::vec3& posi CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm::vec3& endPoint) : Shape(Shape::CAPSULE_SHAPE), _radius(radius), _halfHeight(0.0f) { glm::vec3 axis = endPoint - startPoint; + _position = 0.5f * (endPoint + startPoint); float height = glm::length(axis); if (height > EPSILON) { _halfHeight = 0.5f * height; @@ -50,12 +51,12 @@ CapsuleShape::CapsuleShape(float radius, const glm::vec3& startPoint, const glm: /// \param[out] startPoint is the center of start cap void CapsuleShape::getStartPoint(glm::vec3& startPoint) const { - startPoint = getPosition() - _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); + startPoint = _position - _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); } /// \param[out] endPoint is the center of the end cap void CapsuleShape::getEndPoint(glm::vec3& endPoint) const { - endPoint = getPosition() + _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); + endPoint = _position + _rotation * glm::vec3(0.0f, _halfHeight, 0.0f); } void CapsuleShape::computeNormalizedAxis(glm::vec3& axis) const { diff --git a/libraries/shared/src/CapsuleShape.h b/libraries/shared/src/CapsuleShape.h index 9421bf1789..0889f6b2f3 100644 --- a/libraries/shared/src/CapsuleShape.h +++ b/libraries/shared/src/CapsuleShape.h @@ -14,7 +14,6 @@ #include "Shape.h" -// adebug bookmark TODO: convert to new world-frame approach // default axis of CapsuleShape is Y-axis class CapsuleShape : public Shape { diff --git a/libraries/shared/src/CollisionInfo.cpp b/libraries/shared/src/CollisionInfo.cpp index 5d97842530..22929fbd51 100644 --- a/libraries/shared/src/CollisionInfo.cpp +++ b/libraries/shared/src/CollisionInfo.cpp @@ -23,6 +23,12 @@ CollisionInfo* CollisionList::getNewCollision() { return (_size < _maxSize) ? &(_collisions[_size++]) : NULL; } +void CollisionList::deleteLastCollision() { + if (_size > 0) { + --_size; + } +} + CollisionInfo* CollisionList::getCollision(int index) { return (index > -1 && index < _size) ? &(_collisions[index]) : NULL; } diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h index 510728daa6..f014a31f36 100644 --- a/libraries/shared/src/CollisionInfo.h +++ b/libraries/shared/src/CollisionInfo.h @@ -81,6 +81,9 @@ public: /// \return pointer to next collision. NULL if list is full. CollisionInfo* getNewCollision(); + /// \forget about collision at the end + void deleteLastCollision(); + /// \return pointer to collision by index. NULL if index out of bounds. CollisionInfo* getCollision(int index); diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp index 31d57f14ad..348f8ac97d 100644 --- a/libraries/shared/src/ShapeCollider.cpp +++ b/libraries/shared/src/ShapeCollider.cpp @@ -591,7 +591,95 @@ bool listList(const ListShape* listA, const ListShape* listB, CollisionList& col } // helper function -bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { +bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, + float cubeSide, CollisionList& collisions) { + // sphere is A + // cube is B + // BA = B - A = from center of A to center of B + float halfCubeSide = 0.5f * cubeSide; + glm::vec3 BA = cubeCenter - sphereCenter; + float distance = glm::length(BA); + if (distance > EPSILON) { + float maxBA = glm::max(glm::max(glm::abs(BA.x), glm::abs(BA.y)), glm::abs(BA.z)); + if (maxBA > halfCubeSide + sphereRadius) { + // sphere misses cube entirely + return false; + } + CollisionInfo* collision = collisions.getNewCollision(); + if (!collision) { + return false; + } + if (maxBA > halfCubeSide) { + // sphere hits cube but its center is outside cube + + // compute contact anti-pole on cube (in cube frame) + glm::vec3 cubeContact = glm::abs(BA); + if (cubeContact.x > halfCubeSide) { + cubeContact.x = halfCubeSide; + } + if (cubeContact.y > halfCubeSide) { + cubeContact.y = halfCubeSide; + } + if (cubeContact.z > halfCubeSide) { + cubeContact.z = halfCubeSide; + } + glm::vec3 signs = glm::sign(BA); + cubeContact.x *= signs.x; + cubeContact.y *= signs.y; + cubeContact.z *= signs.z; + + // compute penetration direction + glm::vec3 direction = BA - cubeContact; + float lengthDirection = glm::length(direction); + if (lengthDirection < EPSILON) { + // sphereCenter is touching cube surface, so we can't use the difference between those two + // points to compute the penetration direction. Instead we use the unitary components of + // cubeContact. + direction = cubeContact / halfCubeSide; + glm::modf(BA, direction); + lengthDirection = glm::length(direction); + } else if (lengthDirection > sphereRadius) { + collisions.deleteLastCollision(); + return false; + } + direction /= lengthDirection; + + // compute collision details + collision->_contactPoint = sphereCenter + sphereRadius * direction; + collision->_penetration = sphereRadius * direction - (BA - cubeContact); + } else { + // sphere center is inside cube + // --> push out nearest face + glm::vec3 direction; + BA /= maxBA; + glm::modf(BA, direction); + direction = glm::normalize(direction); + + // compute collision details + collision->_penetration = (halfCubeSide + sphereRadius - distance * glm::dot(BA, direction)) * direction; + collision->_contactPoint = sphereCenter + sphereRadius * direction; + } + return true; + } else if (sphereRadius + halfCubeSide > distance) { + // NOTE: for cocentric approximation we collide sphere and cube as two spheres which means + // this algorithm will probably be wrong when both sphere and cube are very small (both ~EPSILON) + CollisionInfo* collision = collisions.getNewCollision(); + if (collision) { + // the penetration and contactPoint are undefined, so we pick a penetration direction (-yAxis) + collision->_penetration = (sphereRadius + halfCubeSide) * glm::vec3(0.0f, -1.0f, 0.0f); + // contactPoint is on surface of A + collision->_contactPoint = sphereCenter + collision->_penetration; + return true; + } + } + return false; +} + +// helper function +/* KEEP THIS CODE -- this is how to collide the cube with stark face normals (no rounding). +* We might want to use this code later for sealing boundaries between adjacent voxels. +bool sphereAACube_StarkAngles(const glm::vec3& sphereCenter, float sphereRadius, const glm::vec3& cubeCenter, + float cubeSide, CollisionList& collisions) { glm::vec3 BA = cubeCenter - sphereCenter; float distance = glm::length(BA); if (distance > EPSILON) { @@ -606,50 +694,16 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm:: if (glm::dot(surfaceAB, BA) > 0.f) { CollisionInfo* collision = collisions.getNewCollision(); if (collision) { - /* KEEP THIS CODE -- this is how to collide the cube with stark face normals (no rounding). - * We might want to use this code later for sealing boundaries between adjacent voxels. // penetration is parallel to box side direction BA /= maxBA; glm::vec3 direction; glm::modf(BA, direction); direction = glm::normalize(direction); - */ - - // For rounded normals at edges and corners: - // At this point imagine that sphereCenter touches a "normalized" cube with rounded edges. - // This cube has a sidelength of 2 and its smoothing radius is sphereRadius/maxBA. - // We're going to try to compute the "negative normal" (and hence direction of penetration) - // of this surface. - - float radius = sphereRadius / (distance * maxBA); // normalized radius - float shortLength = maxBA - radius; - glm::vec3 direction = BA; - if (shortLength > 0.0f) { - direction = glm::abs(BA) - glm::vec3(shortLength); - // Set any negative components to zero, and adopt the sign of the original BA component. - // Unfortunately there isn't an easy way to make this fast. - if (direction.x < 0.0f) { - direction.x = 0.f; - } else if (BA.x < 0.f) { - direction.x = -direction.x; - } - if (direction.y < 0.0f) { - direction.y = 0.f; - } else if (BA.y < 0.f) { - direction.y = -direction.y; - } - if (direction.z < 0.0f) { - direction.z = 0.f; - } else if (BA.z < 0.f) { - direction.z = -direction.z; - } - } - direction = glm::normalize(direction); // penetration is the projection of surfaceAB on direction collision->_penetration = glm::dot(surfaceAB, direction) * direction; // contactPoint is on surface of A - collision->_contactPoint = sphereCenter - sphereRadius * direction; + collision->_contactPoint = sphereCenter + sphereRadius * direction; return true; } } @@ -667,6 +721,7 @@ bool sphereAACube(const glm::vec3& sphereCenter, float sphereRadius, const glm:: } return false; } +*/ bool sphereAACube(const SphereShape* sphereA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions) { return sphereAACube(sphereA->getPosition(), sphereA->getRadius(), cubeCenter, cubeSide, collisions); diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp index 3f952236e2..7b3d956065 100644 --- a/tests/physics/src/ShapeColliderTests.cpp +++ b/tests/physics/src/ShapeColliderTests.cpp @@ -681,58 +681,164 @@ void ShapeColliderTests::capsuleTouchesCapsule() { } } -void ShapeColliderTests::sphereTouchesAACube() { +void ShapeColliderTests::sphereTouchesAACubeFaces() { CollisionList collisions(16); glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f); + float cubeSide = 2.34f; + + float sphereRadius = 1.13f; + glm::vec3 sphereCenter(0.0f); + SphereShape sphere(sphereRadius, sphereCenter); + + QVector axes; + axes.push_back(xAxis); + axes.push_back(-xAxis); + axes.push_back(yAxis); + axes.push_back(-yAxis); + axes.push_back(zAxis); + axes.push_back(-zAxis); + + for (int i = 0; i < axes.size(); ++i) { + glm::vec3 axis = axes[i]; + // outside + { + collisions.clear(); + float overlap = 0.25f; + float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; + sphereCenter = cubeCenter + sphereOffset * axis; + sphere.setPosition(sphereCenter); + + if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl; + } + CollisionInfo* collision = collisions[0]; + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. axis = " << axis << std::endl; + } + + glm::vec3 expectedPenetration = - overlap * axis; + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration + << " axis = " << axis + << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * axis; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact + << " axis = " << axis + << std::endl; + } + } + + // inside + { + collisions.clear(); + float overlap = 1.25f * sphereRadius; + float sphereOffset = 0.5f * cubeSide + sphereRadius - overlap; + sphereCenter = cubeCenter + sphereOffset * axis; + sphere.setPosition(sphereCenter); + + if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube." + << " axis = " << axis + << std::endl; + } + CollisionInfo* collision = collisions[0]; + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo on y-axis." + << " axis = " << axis + << std::endl; + } + + glm::vec3 expectedPenetration = - overlap * axis; + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration + << " axis = " << axis + << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * axis; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact + << " axis = " << axis + << std::endl; + } + } + } +} + +void ShapeColliderTests::sphereTouchesAACubeEdges() { + CollisionList collisions(20); + + glm::vec3 cubeCenter(0.0f, 0.0f, 0.0f); float cubeSide = 2.0f; float sphereRadius = 1.0f; glm::vec3 sphereCenter(0.0f); SphereShape sphere(sphereRadius, sphereCenter); - float sphereOffset = (0.5f * cubeSide + sphereRadius - 0.25f); + QVector axes; + // edges + axes.push_back(glm::vec3(0.0f, 1.0f, 1.0f)); + axes.push_back(glm::vec3(0.0f, 1.0f, -1.0f)); + axes.push_back(glm::vec3(0.0f, -1.0f, 1.0f)); + axes.push_back(glm::vec3(0.0f, -1.0f, -1.0f)); + axes.push_back(glm::vec3(1.0f, 1.0f, 0.0f)); + axes.push_back(glm::vec3(1.0f, -1.0f, 0.0f)); + axes.push_back(glm::vec3(-1.0f, 1.0f, 0.0f)); + axes.push_back(glm::vec3(-1.0f, -1.0f, 0.0f)); + axes.push_back(glm::vec3(1.0f, 0.0f, 1.0f)); + axes.push_back(glm::vec3(1.0f, 0.0f, -1.0f)); + axes.push_back(glm::vec3(-1.0f, 0.0f, 1.0f)); + axes.push_back(glm::vec3(-1.0f, 0.0f, -1.0f)); + // and corners + axes.push_back(glm::vec3(1.0f, 1.0f, 1.0f)); + axes.push_back(glm::vec3(1.0f, 1.0f, -1.0f)); + axes.push_back(glm::vec3(1.0f, -1.0f, 1.0f)); + axes.push_back(glm::vec3(1.0f, -1.0f, -1.0f)); + axes.push_back(glm::vec3(-1.0f, 1.0f, 1.0f)); + axes.push_back(glm::vec3(-1.0f, 1.0f, -1.0f)); + axes.push_back(glm::vec3(-1.0f, -1.0f, 1.0f)); + axes.push_back(glm::vec3(-1.0f, -1.0f, -1.0f)); - // top - sphereCenter = cubeCenter + sphereOffset * yAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } + for (int i =0; i < axes.size(); ++i) { + glm::vec3 axis = axes[i]; + float lengthAxis = glm::length(axis); + axis /= lengthAxis; + float overlap = 0.25f; - // bottom - sphereCenter = cubeCenter - sphereOffset * yAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } - - // left - sphereCenter = cubeCenter + sphereOffset * xAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } - - // right - sphereCenter = cubeCenter - sphereOffset * xAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } - - // forward - sphereCenter = cubeCenter + sphereOffset * zAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; - } - - // back - sphereCenter = cubeCenter - sphereOffset * zAxis; - sphere.setPosition(sphereCenter); - if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ - std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube" << std::endl; + sphereCenter = cubeCenter + (lengthAxis * 0.5f * cubeSide + sphereRadius - overlap) * axis; + sphere.setPosition(sphereCenter); + + if (!ShapeCollider::sphereAACube(&sphere, cubeCenter, cubeSide, collisions)){ + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should collide with cube. axis = " << axis << std::endl; + } + CollisionInfo* collision = collisions[i]; + if (!collision) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: no CollisionInfo. axis = " << axis << std::endl; + } + + glm::vec3 expectedPenetration = - overlap * axis; + if (glm::distance(expectedPenetration, collision->_penetration) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: penetration = " << collision->_penetration + << " expected " << expectedPenetration + << " axis = " << axis + << std::endl; + } + + glm::vec3 expectedContact = sphereCenter - sphereRadius * axis; + if (glm::distance(expectedContact, collision->_contactPoint) > EPSILON) { + std::cout << __FILE__ << ":" << __LINE__ << " ERROR: contactaPoint = " << collision->_contactPoint + << " expected " << expectedContact + << " axis = " << axis + << std::endl; + } } } @@ -802,6 +908,7 @@ void ShapeColliderTests::runAllTests() { capsuleMissesCapsule(); capsuleTouchesCapsule(); - sphereTouchesAACube(); + sphereTouchesAACubeFaces(); + sphereTouchesAACubeEdges(); sphereMissesAACube(); } diff --git a/tests/physics/src/ShapeColliderTests.h b/tests/physics/src/ShapeColliderTests.h index a94f5050ff..b51c48a61e 100644 --- a/tests/physics/src/ShapeColliderTests.h +++ b/tests/physics/src/ShapeColliderTests.h @@ -23,7 +23,8 @@ namespace ShapeColliderTests { void capsuleMissesCapsule(); void capsuleTouchesCapsule(); - void sphereTouchesAACube(); + void sphereTouchesAACubeFaces(); + void sphereTouchesAACubeEdges(); void sphereMissesAACube(); void runAllTests();