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

Merge pull request #3420 from AndrewMeadows/ragdoll

Browse source 
Walking avatar tweaks
This commit is contained in:
Philip Rosedale 2014-09-15 16:12:23 -07:00
commit bd5cb346d2
30 changed files with 978 additions and 462 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
interface/src/Menu.cpp
View file

@ -277,7 +277,8 @@ Menu::Menu() :
avatar, SLOT(updateMotionBehaviorsFromMenu()));
QMenu* collisionsMenu = avatarMenu->addMenu("Collide With...");
addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideAsRagdoll);
addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideAsRagdoll, 0, false,
avatar, SLOT(onToggleRagdoll()));
addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideWithAvatars,
0, true, avatar, SLOT(updateCollisionGroups()));
addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideWithVoxels,
@ -745,6 +746,7 @@ void Menu::loadSettings(QSettings* settings) {
// TODO: cache more settings in MyAvatar that are checked with very high frequency.
MyAvatar* myAvatar = Application::getInstance()->getAvatar();
myAvatar->updateCollisionGroups();
myAvatar->onToggleRagdoll();
if (lockedSettings) {
Application::getInstance()->unlockSettings();

6
interface/src/MetavoxelSystem.cpp
View file

@ -2479,9 +2479,9 @@ void StaticModelRenderer::renderUnclipped(float alpha, Mode mode) {
_model->render(alpha);
}
bool StaticModelRenderer::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
const glm::vec3& clipMinimum, float clipSize, float& distance) const {
return _model->findRayIntersection(origin, direction, distance);
bool StaticModelRenderer::findRayIntersection(RayIntersectionInfo& intersection,
const glm::vec3& clipMinimum, float clipSize) const {
return _model->findRayIntersection(intersection);
}
void StaticModelRenderer::applyTranslation(const glm::vec3& translation) {

4
interface/src/MetavoxelSystem.h
View file

@ -392,8 +392,8 @@ public:
virtual void init(Spanner* spanner);
virtual void simulate(float deltaTime);
virtual bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
const glm::vec3& clipMinimum, float clipSize, float& distance) const;
virtual bool findRayIntersection(RayIntersectionInfo& intersection,
const glm::vec3& clipMinimum, float clipSize) const;
protected:

18
interface/src/avatar/Avatar.cpp
View file

@ -715,20 +715,10 @@ void Avatar::renderDisplayName() {
glEnable(GL_LIGHTING);
}
bool Avatar::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const {
float minDistance = FLT_MAX;
float modelDistance;
if (_skeletonModel.findRayIntersection(origin, direction, modelDistance)) {
minDistance = qMin(minDistance, modelDistance);
}
if (getHead()->getFaceModel().findRayIntersection(origin, direction, modelDistance)) {
minDistance = qMin(minDistance, modelDistance);
}
if (minDistance < FLT_MAX) {
distance = minDistance;
return true;
}
return false;
bool Avatar::findRayIntersection(RayIntersectionInfo& intersection) const {
bool hit = _skeletonModel.findRayIntersection(intersection);
hit = getHead()->getFaceModel().findRayIntersection(intersection) || hit;
return hit;
}
bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius, CollisionList& collisions) {

2
interface/src/avatar/Avatar.h
View file

@ -99,7 +99,7 @@ public:
/// Returns the distance to use as a LOD parameter.
float getLODDistance() const;
bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
bool findRayIntersection(RayIntersectionInfo& intersection) const;
/// \param shapes list of shapes to collide against avatar
/// \param collisions list to store collision results

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

@ -49,7 +49,7 @@ 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 MIN_KEYBOARD_CONTROL_SPEED = 2.0f;
const float MIN_KEYBOARD_CONTROL_SPEED = 1.5f;
const float MAX_WALKING_SPEED = 3.0f * MIN_KEYBOARD_CONTROL_SPEED;
// TODO: normalize avatar speed for standard avatar size, then scale all motion logic
@ -75,7 +75,6 @@ MyAvatar::MyAvatar() :
_motorTimescale(DEFAULT_MOTOR_TIMESCALE),
_maxMotorSpeed(MAX_MOTOR_SPEED),
_motionBehaviors(AVATAR_MOTION_DEFAULTS),
_lastFloorContactPoint(0.0f),
_lookAtTargetAvatar(),
_shouldRender(true),
_billboardValid(false),
@ -87,11 +86,10 @@ MyAvatar::MyAvatar() :
_driveKeys[i] = 0.0f;
}
_physicsSimulation.setEntity(&_skeletonModel);
_physicsSimulation.addEntity(&_voxelShapeManager);
_skeletonModel.setEnableShapes(true);
Ragdoll* ragdoll = _skeletonModel.buildRagdoll();
_physicsSimulation.setRagdoll(ragdoll);
_physicsSimulation.addEntity(&_voxelShapeManager);
_skeletonModel.buildRagdoll();
// connect to AddressManager signal for location jumps
connect(&AddressManager::getInstance(), &AddressManager::locationChangeRequired, this, &MyAvatar::goToLocation);
@ -217,15 +215,15 @@ void MyAvatar::simulate(float deltaTime) {
}
{
PerformanceTimer perfTimer("ragdoll");
PerformanceTimer perfTimer("physics");
const float minError = 0.00001f;
const float maxIterations = 3;
const quint64 maxUsec = 4000;
_physicsSimulation.setTranslation(_position);
_physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec);
Ragdoll* ragdoll = _skeletonModel.getRagdoll();
if (ragdoll && Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
const float minError = 0.00001f;
const float maxIterations = 3;
const quint64 maxUsec = 4000;
_physicsSimulation.setTranslation(_position);
_physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec);
// harvest any displacement of the Ragdoll that is a result of collisions
glm::vec3 ragdollDisplacement = ragdoll->getAndClearAccumulatedMovement();
const float MAX_RAGDOLL_DISPLACEMENT_2 = 1.0f;
@ -1086,15 +1084,6 @@ bool MyAvatar::shouldRenderHead(const glm::vec3& cameraPosition, RenderMode rend
(glm::length(cameraPosition - head->getEyePosition()) > RENDER_HEAD_CUTOFF_DISTANCE * _scale);
}
float MyAvatar::computeDistanceToFloor(const glm::vec3& startPoint) {
glm::vec3 direction = -_worldUpDirection;
OctreeElement* elementHit; // output from findRayIntersection
float distance = FLT_MAX; // output from findRayIntersection
BoxFace face; // output from findRayIntersection
Application::getInstance()->getVoxelTree()->findRayIntersection(startPoint, direction, elementHit, distance, face);
return distance;
}
void MyAvatar::updateOrientation(float deltaTime) {
// Gather rotation information from keyboard
_bodyYawDelta -= _driveKeys[ROT_RIGHT] * YAW_SPEED * deltaTime;
@ -1152,86 +1141,69 @@ void MyAvatar::updateOrientation(float deltaTime) {
const float NEARBY_FLOOR_THRESHOLD = 5.0f;
void MyAvatar::updatePosition(float 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) * GRAVITY_EARTH;
// check for floor by casting a ray straight down from avatar's position
float heightAboveFloor = FLT_MAX;
const CapsuleShape& boundingShape = _skeletonModel.getBoundingShape();
glm::vec3 startCap;
boundingShape.getStartPoint(startCap);
glm::vec3 bottom = startCap - boundingShape.getRadius() * _worldUpDirection;
RayIntersectionInfo intersection;
// NOTE: avatar is center of PhysicsSimulation, so rayStart is the origin for the purposes of the raycast
intersection._rayStart = glm::vec3(0.0f);
intersection._rayDirection = - _worldUpDirection;
intersection._rayLength = 5.0f * boundingShape.getBoundingRadius();
if (_physicsSimulation.findFloorRayIntersection(intersection)) {
// NOTE: heightAboveFloor is the distance between the bottom of the avatar and the floor
heightAboveFloor = intersection._hitDistance - boundingShape.getBoundingRadius();
}
// velocity is initialized to the measured _velocity but will be modified
// by friction, external thrust, etc
// velocity is initialized to the measured _velocity but will be modified by friction, external thrust, etc
glm::vec3 velocity = _velocity;
// apply friction
if (gravityLength > EPSILON) {
float speedFromGravity = _scale * deltaTime * gravityLength;
float distanceToFall = glm::distance(bottom, _lastFloorContactPoint);
walkingOnFloor = (distanceToFall < 2.0f * deltaTime * speedFromGravity);
if (walkingOnFloor) {
// BEGIN HACK: to prevent the avatar from bouncing on a floor surface
if (distanceToFall < deltaTime * speedFromGravity) {
float verticalSpeed = glm::dot(velocity, _worldUpDirection);
if (fabs(verticalSpeed) < speedFromGravity) {
// we're standing on a floor, and nearly at rest so we zero the vertical velocity component
velocity -= verticalSpeed * _worldUpDirection;
}
} else {
// fall with gravity against floor
velocity -= speedFromGravity * _worldUpDirection;
}
// END HACK
bool pushingUp = (_driveKeys[UP] - _driveKeys[DOWN] > 0.0f);
bool walkingOnFloor = false;
if (_motionBehaviors & AVATAR_MOTION_STAND_ON_NEARBY_FLOORS) {
const float MAX_SPEED_UNDER_GRAVITY = 2.0f * _scale * MAX_WALKING_SPEED;
if (pushingUp || glm::length2(velocity) > MAX_SPEED_UNDER_GRAVITY * MAX_SPEED_UNDER_GRAVITY) {
// we're pushing up or moving quickly, so disable gravity
setLocalGravity(glm::vec3(0.0f));
} else {
if (!_isBraking) {
// fall with gravity toward floor
velocity -= speedFromGravity * _worldUpDirection;
}
if (_motionBehaviors & AVATAR_MOTION_STAND_ON_NEARBY_FLOORS) {
const float MAX_VERTICAL_FLOOR_DETECTION_SPEED = _scale * MAX_WALKING_SPEED;
if (keyboardInput && glm::dot(_motorVelocity, _worldUpDirection) > 0.0f &&
glm::dot(velocity, _worldUpDirection) > MAX_VERTICAL_FLOOR_DETECTION_SPEED) {
// disable local gravity when flying up
setLocalGravity(glm::vec3(0.0f));
} else {
const float maxFloorDistance = _scale * NEARBY_FLOOR_THRESHOLD;
if (computeDistanceToFloor(bottom) > maxFloorDistance) {
// disable local gravity when floor is too far
setLocalGravity(glm::vec3(0.0f));
}
}
}
}
} else if ((_collisionGroups & COLLISION_GROUP_VOXELS) &&
_motionBehaviors & AVATAR_MOTION_STAND_ON_NEARBY_FLOORS) {
const float MIN_FLOOR_DETECTION_SPEED = _scale * 1.0f;
if (glm::length(_velocity) < MIN_FLOOR_DETECTION_SPEED ) {
// scan for floor under avatar
const float maxFloorDistance = _scale * NEARBY_FLOOR_THRESHOLD;
if (computeDistanceToFloor(bottom) < maxFloorDistance) {
// enable local gravity
const float maxFloorDistance = boundingShape.getBoundingRadius() * NEARBY_FLOOR_THRESHOLD;
if (heightAboveFloor > maxFloorDistance) {
// disable local gravity when floor is too far away
setLocalGravity(glm::vec3(0.0f));
} else {
// enable gravity
walkingOnFloor = true;
setLocalGravity(-_worldUpDirection);
}
}
}
float speed = glm::length(velocity);
if (keyboardInput > 0.0f || speed > 0.0f || glm::length2(_thrust) > 0.0f || ! walkingOnFloor) {
// update motor
if (_motionBehaviors & AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED) {
// Increase motor velocity until its length is equal to _maxMotorSpeed.
glm::vec3 localVelocity = velocity;
if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) {
glm::quat orientation = getHead()->getCameraOrientation();
localVelocity = glm::inverse(orientation) * velocity;
}
bool zeroDownwardVelocity = false;
bool gravityEnabled = (glm::length2(_gravity) > EPSILON);
if (gravityEnabled) {
if (heightAboveFloor < 0.0f) {
// Gravity is in effect so we assume that the avatar is colliding against the world and we need
// to lift avatar out of floor, but we don't want to do it too fast (keep it smooth).
float distanceToLift = glm::min(-heightAboveFloor, MAX_WALKING_SPEED * deltaTime);
// We don't use applyPositionDelta() for this lift distance because we don't want the avatar
// to come flying out of the floor. Instead we update position directly, and set a boolean
// that will remind us later to zero any downward component of the velocity.
_position += (distanceToLift - EPSILON) * _worldUpDirection;
zeroDownwardVelocity = true;
}
velocity += (deltaTime * GRAVITY_EARTH) * _gravity;
}
float motorEfficiency = glm::clamp(deltaTime / computeMotorTimescale(velocity), 0.0f, 1.0f);
// compute targetVelocity
glm::vec3 targetVelocity(0.0f);
if (_motionBehaviors & AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED) {
float keyboardInput = fabsf(_driveKeys[FWD] - _driveKeys[BACK]) +
(fabsf(_driveKeys[RIGHT] - _driveKeys[LEFT])) +
fabsf(_driveKeys[UP] - _driveKeys[DOWN]);
if (keyboardInput) {
// Compute keyboard input
glm::vec3 front = (_driveKeys[FWD] - _driveKeys[BACK]) * IDENTITY_FRONT;
glm::vec3 right = (_driveKeys[RIGHT] - _driveKeys[LEFT]) * IDENTITY_RIGHT;
@ -1243,76 +1215,69 @@ void MyAvatar::updatePosition(float deltaTime) {
// Compute motor magnitude
if (directionLength > EPSILON) {
direction /= directionLength;
// the finalMotorSpeed depends on whether we are walking or not
// Compute the target keyboard velocity (which ramps up slowly, and damps very quickly)
// the max magnitude of which depends on what we're doing:
float finalMaxMotorSpeed = walkingOnFloor ? _scale * MAX_WALKING_SPEED : _scale * _maxMotorSpeed;
float motorLength = glm::length(_motorVelocity);
if (motorLength < _scale * MIN_KEYBOARD_CONTROL_SPEED) {
// an active keyboard motor should never be slower than this
_motorVelocity = _scale * MIN_KEYBOARD_CONTROL_SPEED * direction;
motorEfficiency = 1.0f;
} 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;
float MOTOR_LENGTH_TIMESCALE = 2.0f;
float INCREASE_FACTOR = 1.8f;
motorLength *= 1.0f + glm::clamp(deltaTime / MOTOR_LENGTH_TIMESCALE, 0.0f, 1.0f) * INCREASE_FACTOR;
if (motorLength > finalMaxMotorSpeed) {
motorLength = finalMaxMotorSpeed;
}
_motorVelocity = motorLength * direction;
}
_isPushing = true;
} else {
// motor opposes motion (wants to be at rest)
_motorVelocity = - localVelocity;
}
}
targetVelocity = _motorVelocity;
} else {
_motorVelocity = glm::vec3(0.0f);
}
}
targetVelocity = getHead()->getCameraOrientation() * targetVelocity;
// apply motor
if (_motionBehaviors & AVATAR_MOTION_MOTOR_ENABLED) {
glm::vec3 targetVelocity = _motorVelocity;
if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) {
// rotate targetVelocity into world frame
glm::quat rotation = getHead()->getCameraOrientation();
targetVelocity = rotation * _motorVelocity;
}
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(velocity);
float tau = glm::clamp(deltaTime / timescale, 0.0f, 1.0f);
velocity += tau * deltaVelocity;
}
glm::vec3 deltaVelocity = targetVelocity - velocity;
// apply thrust
velocity += _thrust * deltaTime;
speed = glm::length(velocity);
if (speed > MAX_AVATAR_SPEED) {
velocity *= MAX_AVATAR_SPEED / speed;
speed = MAX_AVATAR_SPEED;
}
_thrust = glm::vec3(0.0f);
if (walkingOnFloor && !pushingUp) {
// remove vertical component of deltaVelocity
deltaVelocity -= glm::dot(deltaVelocity, _worldUpDirection) * _worldUpDirection;
}
// update position
const float MIN_AVATAR_SPEED = 0.075f;
if (speed > MIN_AVATAR_SPEED) {
applyPositionDelta(deltaTime * velocity);
// apply motor
velocity += motorEfficiency * deltaVelocity;
// apply thrust
velocity += _thrust * deltaTime;
_thrust = glm::vec3(0.0f);
// remove downward velocity so we don't push into floor
if (zeroDownwardVelocity) {
float verticalSpeed = glm::dot(velocity, _worldUpDirection);
if (verticalSpeed < 0.0f) {
velocity += verticalSpeed * _worldUpDirection;
}
}
// update moving flag based on speed
// cap avatar speed
float speed = glm::length(velocity);
if (speed > MAX_AVATAR_SPEED) {
velocity *= MAX_AVATAR_SPEED / speed;
speed = MAX_AVATAR_SPEED;
}
// update position
const float MIN_AVATAR_SPEED = 0.075f;
if (speed > MIN_AVATAR_SPEED) {
applyPositionDelta(deltaTime * velocity);
}
// update _moving flag based on speed
const float MOVING_SPEED_THRESHOLD = 0.01f;
_moving = speed > MOVING_SPEED_THRESHOLD;
@ -1331,8 +1296,8 @@ float MyAvatar::computeMotorTimescale(const glm::vec3& velocity) {
// (3) inactive --> long timescale (gentle friction for low speeds)
float MIN_MOTOR_TIMESCALE = 0.125f;
float MAX_MOTOR_TIMESCALE = 0.5f;
float MIN_BRAKE_SPEED = 0.4f;
float MAX_MOTOR_TIMESCALE = 0.4f;
float MIN_BRAKE_SPEED = 0.3f;
float timescale = MAX_MOTOR_TIMESCALE;
bool isThrust = (glm::length2(_thrust) > EPSILON);
@ -1369,18 +1334,23 @@ void MyAvatar::updateCollisionWithEnvironment(float deltaTime, float radius) {
static CollisionList myCollisions(64);
void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
quint64 now = usecTimestampNow();
if (_voxelShapeManager.needsUpdate(now)) {
// We use a multiple of the avatar's boundingRadius as the size of the cube of interest.
float cubeScale = 4.0f * getBoundingRadius();
float cubeScale = 6.0f * getBoundingRadius();
glm::vec3 corner = getPosition() - glm::vec3(0.5f * cubeScale);
AACube boundingCube(corner, cubeScale);
// query the VoxelTree for cubes that touch avatar's boundingCube
CubeList cubes;
if (Application::getInstance()->getVoxelTree()->findContentInCube(boundingCube, cubes)) {
_voxelShapeManager.updateVoxels(cubes);
_voxelShapeManager.updateVoxels(now, cubes);
}
} else {
}
// TODO: Andrew to do ground/walking detection in ragdoll mode
if (!Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
const float MAX_VOXEL_COLLISION_SPEED = 100.0f;
float speed = glm::length(_velocity);
if (speed > MAX_VOXEL_COLLISION_SPEED) {
@ -1390,15 +1360,18 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
}
bool isTrapped = false;
myCollisions.clear();
const CapsuleShape& boundingShape = _skeletonModel.getBoundingShape();
if (Application::getInstance()->getVoxelTree()->findShapeCollisions(&boundingShape, myCollisions, Octree::TryLock)) {
// copy the boundingShape and tranform into physicsSimulation frame
CapsuleShape boundingShape = _skeletonModel.getBoundingShape();
boundingShape.setTranslation(boundingShape.getTranslation() - _position);
if (_physicsSimulation.getShapeCollisions(&boundingShape, myCollisions)) {
// we temporarily move b
const float VOXEL_ELASTICITY = 0.0f;
const float VOXEL_DAMPING = 0.0f;
float capsuleRadius = boundingShape.getRadius();
float capsuleHalfHeight = boundingShape.getHalfHeight();
const float capsuleRadius = boundingShape.getRadius();
const float capsuleHalfHeight = boundingShape.getHalfHeight();
const float MAX_STEP_HEIGHT = capsuleRadius + capsuleHalfHeight;
const float MIN_STEP_HEIGHT = 0.0f;
glm::vec3 footBase = boundingShape.getTranslation() - (capsuleRadius + capsuleHalfHeight) * _worldUpDirection;
float highestStep = 0.0f;
float lowestStep = MAX_STEP_HEIGHT;
glm::vec3 floorPoint;
@ -1407,43 +1380,51 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
for (int i = 0; i < myCollisions.size(); ++i) {
CollisionInfo* collision = myCollisions[i];
glm::vec3 cubeCenter = collision->_vecData;
float cubeSide = collision->_floatData;
float verticalDepth = glm::dot(collision->_penetration, _worldUpDirection);
float horizontalDepth = glm::length(collision->_penetration - verticalDepth * _worldUpDirection);
const float MAX_TRAP_PERIOD = 0.125f;
if (horizontalDepth > capsuleRadius || fabsf(verticalDepth) > MAX_STEP_HEIGHT) {
isTrapped = true;
if (_trapDuration > MAX_TRAP_PERIOD) {
float distance = glm::dot(boundingShape.getTranslation() - cubeCenter, _worldUpDirection);
if (distance < 0.0f) {
distance = fabsf(distance) + 0.5f * cubeSide;
RayIntersectionInfo intersection;
// we pick a rayStart that we expect to be inside the boundingShape (aka shapeA)
intersection._rayStart = collision->_contactPoint - MAX_STEP_HEIGHT * glm::normalize(collision->_penetration);
intersection._rayDirection = -_worldUpDirection;
// cast the ray down against shapeA
if (collision->_shapeA->findRayIntersection(intersection)) {
float firstDepth = - intersection._hitDistance;
// recycle intersection and cast again in up against shapeB
intersection._rayDirection = _worldUpDirection;
intersection._hitDistance = FLT_MAX;
if (collision->_shapeB->findRayIntersection(intersection)) {
// now we know how much we need to move UP to get out
totalPenetration = addPenetrations(totalPenetration,
(firstDepth + intersection._hitDistance) * _worldUpDirection);
}
}
distance += capsuleRadius + capsuleHalfHeight;
totalPenetration = addPenetrations(totalPenetration, - distance * _worldUpDirection);
continue;
}
} else if (_trapDuration > MAX_TRAP_PERIOD) {
// we're trapped, ignore this collision
// we're trapped, ignore this shallow collision
continue;
}
totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
// some logic to help us walk up steps
if (glm::dot(collision->_penetration, _velocity) >= 0.0f) {
glm::vec3 cubeTop = cubeCenter + (0.5f * cubeSide) * _worldUpDirection;
float stepHeight = glm::dot(_worldUpDirection, cubeTop - footBase);
float stepHeight = - glm::dot(_worldUpDirection, collision->_penetration);
if (stepHeight > highestStep) {
highestStep = stepHeight;
stepPenetration = collision->_penetration;
}
if (stepHeight < lowestStep) {
lowestStep = stepHeight;
floorPoint = collision->_contactPoint - collision->_penetration;
// remember that collision is in _physicsSimulation frame so we must add _position
floorPoint = _position + collision->_contactPoint - collision->_penetration;
}
}
}
if (lowestStep < MAX_STEP_HEIGHT) {
_lastFloorContactPoint = floorPoint;
}
float penetrationLength = glm::length(totalPenetration);
if (penetrationLength < EPSILON) {
@ -1453,12 +1434,11 @@ void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
float verticalPenetration = glm::dot(totalPenetration, _worldUpDirection);
if (highestStep > MIN_STEP_HEIGHT && highestStep < MAX_STEP_HEIGHT && verticalPenetration <= 0.0f) {
// we're colliding against an edge
// rotate _motorVelocity into world frame
glm::vec3 targetVelocity = _motorVelocity;
if (_motionBehaviors & AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME) {
// rotate _motorVelocity into world frame
glm::quat rotation = getHead()->getCameraOrientation();
targetVelocity = rotation * _motorVelocity;
}
glm::quat rotation = getHead()->getCameraOrientation();
targetVelocity = rotation * _motorVelocity;
if (_wasPushing && glm::dot(targetVelocity, totalPenetration) > EPSILON) {
// we're puhing into the edge, so we want to lift
@ -1834,6 +1814,17 @@ void MyAvatar::updateMotionBehaviorsFromMenu() {
}
}
void MyAvatar::onToggleRagdoll() {
Ragdoll* ragdoll = _skeletonModel.getRagdoll();
if (ragdoll) {
if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
_physicsSimulation.setRagdoll(ragdoll);
} else {
_physicsSimulation.setRagdoll(NULL);
}
}
}
void MyAvatar::renderAttachments(RenderMode renderMode) {
if (Application::getInstance()->getCamera()->getMode() != CAMERA_MODE_FIRST_PERSON || renderMode == MIRROR_RENDER_MODE) {
Avatar::renderAttachments(renderMode);

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

@ -164,6 +164,7 @@ public slots:
void setVelocity(const glm::vec3 velocity) { _velocity = velocity; }
void updateMotionBehaviorsFromMenu();
void onToggleRagdoll();
glm::vec3 getLeftPalmPosition();
glm::vec3 getRightPalmPosition();
@ -206,7 +207,6 @@ private:
float _maxMotorSpeed;
quint32 _motionBehaviors;
glm::vec3 _lastFloorContactPoint;
QWeakPointer<AvatarData> _lookAtTargetAvatar;
glm::vec3 _targetAvatarPosition;
bool _shouldRender;
@ -220,7 +220,6 @@ private:
RecorderPointer _recorder;
// private methods
float computeDistanceToFloor(const glm::vec3& startPoint);
void updateOrientation(float deltaTime);
void updatePosition(float deltaTime);
float computeMotorTimescale(const glm::vec3& velocity);

6
interface/src/avatar/VoxelShapeManager.cpp
View file

@ -17,7 +17,7 @@
#include "VoxelShapeManager.h"
VoxelShapeManager::VoxelShapeManager() : PhysicsEntity(), _lastSimulationTranslation(0.0f) {
VoxelShapeManager::VoxelShapeManager() : PhysicsEntity(), _updateExpiry(0), _lastSimulationTranslation(0.0f) {
}
VoxelShapeManager::~VoxelShapeManager() {
@ -57,7 +57,9 @@ void VoxelShapeManager::clearShapes() {
_voxels.clear();
}
void VoxelShapeManager::updateVoxels(CubeList& cubes) {
void VoxelShapeManager::updateVoxels(const quint64& now, CubeList& cubes) {
const quint64 VOXEL_UPDATE_PERIOD = 100000; // usec
_updateExpiry = now + VOXEL_UPDATE_PERIOD;
PhysicsSimulation* simulation = getSimulation();
if (!simulation) {
return;

7
interface/src/avatar/VoxelShapeManager.h
View file

@ -28,7 +28,7 @@ public:
AACubeShape* _shape;
};
typedef QHash<quint64, VoxelInfo> VoxelPool;
typedef QHash<uint, VoxelInfo> VoxelPool;
class VoxelShapeManager : public PhysicsEntity {
public:
@ -39,11 +39,14 @@ public:
void buildShapes();
void clearShapes();
bool needsUpdate(const quint64& now) const { return _updateExpiry < now; }
/// \param cubes list of AACubes representing all of the voxels that should be in this VoxelShapeManager
void updateVoxels(CubeList& cubes);
void updateVoxels(const quint64& now, CubeList& cubes);
private:
quint64 _updateExpiry;
glm::vec3 _lastSimulationTranslation;
VoxelPool _voxels;
};

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

@ -55,20 +55,14 @@ typedef unsigned long long quint64;
#include "HandData.h"
// avatar motion behaviors
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_MOTOR_KEYBOARD_ENABLED = 1U << 0;
const quint32 AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY = 1U << 4;
const quint32 AVATAR_MOTION_OBEY_LOCAL_GRAVITY = 1U << 5;
const quint32 AVATAR_MOTION_STAND_ON_NEARBY_FLOORS = 1U << 6;
const quint32 AVATAR_MOTION_OBEY_ENVIRONMENTAL_GRAVITY = 1U << 1;
const quint32 AVATAR_MOTION_OBEY_LOCAL_GRAVITY = 1U << 2;
const quint32 AVATAR_MOTION_STAND_ON_NEARBY_FLOORS = 1U << 3;
const quint32 AVATAR_MOTION_DEFAULTS =
AVATAR_MOTION_MOTOR_ENABLED |
AVATAR_MOTION_MOTOR_KEYBOARD_ENABLED |
AVATAR_MOTION_MOTOR_USE_LOCAL_FRAME |
AVATAR_MOTION_STAND_ON_NEARBY_FLOORS;
// these bits will be expanded as features are exposed

11
libraries/octree/src/Octree.cpp
View file

@ -851,7 +851,7 @@ bool findShapeCollisionsOp(OctreeElement* element, void* extraData) {
return false;
}
quint64 cubeListHashKey(const glm::vec3& point) {
uint qHash(const glm::vec3& point) {
// NOTE: TREE_SCALE = 16384 (15 bits) and multiplier is 1024 (11 bits),
// so each component (26 bits) uses more than its alloted 21 bits.
// however we don't expect to span huge cubes so it is ok if we wrap
@ -859,9 +859,9 @@ quint64 cubeListHashKey(const glm::vec3& point) {
const uint BITS_PER_COMPONENT = 21;
const quint64 MAX_SCALED_COMPONENT = 2097152; // 2^21
const float RESOLUTION_PER_METER = 1024.0f; // 2^10
return (quint64)(point.x * RESOLUTION_PER_METER) % MAX_SCALED_COMPONENT +
return qHash((quint64)(point.x * RESOLUTION_PER_METER) % MAX_SCALED_COMPONENT +
(((quint64)(point.y * RESOLUTION_PER_METER)) % MAX_SCALED_COMPONENT << BITS_PER_COMPONENT) +
(((quint64)(point.z * RESOLUTION_PER_METER)) % MAX_SCALED_COMPONENT << 2 * BITS_PER_COMPONENT);
(((quint64)(point.z * RESOLUTION_PER_METER)) % MAX_SCALED_COMPONENT << 2 * BITS_PER_COMPONENT));
}
bool findContentInCubeOp(OctreeElement* element, void* extraData) {
@ -877,8 +877,9 @@ bool findContentInCubeOp(OctreeElement* element, void* extraData) {
return true; // recurse on children
}
if (element->hasContent()) {
// NOTE: the voxel's center is unique so we use it as the input for the key
args->cubes->insert(cubeListHashKey(cube.calcCenter()), cube);
// NOTE: the voxel's center is unique so we use it as the input for the key.
// We use the qHash(glm::vec()) as the key as an optimization for the code that uses CubeLists.
args->cubes->insert(qHash(cube.calcCenter()), cube);
return true;
}
return false;

2
libraries/octree/src/Octree.h
View file

@ -48,7 +48,7 @@ public:
// Callback function, for recuseTreeWithOperation
typedef bool (*RecurseOctreeOperation)(OctreeElement* element, void* extraData);
typedef enum {GRADIENT, RANDOM, NATURAL} creationMode;
typedef QHash<quint64, AACube> CubeList;
typedef QHash<uint, AACube> CubeList;
const bool NO_EXISTS_BITS = false;
const bool WANT_EXISTS_BITS = true;

66
libraries/shared/src/AACubeShape.cpp
View file

@ -9,8 +9,68 @@
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#include "AACubeShape.h"
#include <glm/glm.hpp>
#include <glm/gtx/norm.hpp>
bool AACubeShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
return false;
#include "AACubeShape.h"
#include "SharedUtil.h" // for SQUARE_ROOT_OF_3
glm::vec3 faceNormals[3] = { glm::vec3(1.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f), glm::vec3(0.0f, 0.0f, 1.0f) };
bool AACubeShape::findRayIntersection(RayIntersectionInfo& intersection) const {
// A = ray point
// B = cube center
glm::vec3 BA = _translation - intersection._rayStart;
// check for ray intersection with cube's bounding sphere
// a = distance along line to closest approach to B
float a = glm::dot(intersection._rayDirection, BA);
// b2 = squared distance from cube center to point of closest approach
float b2 = glm::length2(a * intersection._rayDirection - BA);
// r = bounding radius of cube
float halfSide = 0.5f * _scale;
const float r = SQUARE_ROOT_OF_3 * halfSide;
if (b2 > r * r) {
// line doesn't hit cube's bounding sphere
return false;
}
// check for tuncated/short ray
// maxLength = maximum possible distance between rayStart and center of cube
const float maxLength = glm::min(intersection._rayLength, intersection._hitDistance) + r;
if (a * a + b2 > maxLength * maxLength) {
// ray is not long enough to reach cube's bounding sphere
// NOTE: we don't fall in here when ray's length if FLT_MAX because maxLength^2 will be inf or nan
return false;
}
// the trivial checks have been exhausted, so must trace to each face
bool hit = false;
for (int i = 0; i < 3; ++i) {
for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
glm::vec3 faceNormal = sign * faceNormals[i];
float rayDotPlane = glm::dot(intersection._rayDirection, faceNormal);
if (glm::abs(rayDotPlane) > EPSILON) {
float distanceToFace = (halfSide + glm::dot(BA, faceNormal)) / rayDotPlane;
if (distanceToFace >= 0.0f) {
glm::vec3 point = distanceToFace * intersection._rayDirection - BA;
int j = (i + 1) % 3;
int k = (i + 2) % 3;
glm::vec3 secondNormal = faceNormals[j];
glm::vec3 thirdNormal = faceNormals[k];
if (glm::abs(glm::dot(point, secondNormal)) > halfSide ||
glm::abs(glm::dot(point, thirdNormal)) > halfSide) {
continue;
}
if (distanceToFace < intersection._hitDistance && distanceToFace < intersection._rayLength) {
intersection._hitDistance = distanceToFace;
intersection._hitNormal = faceNormal;
intersection._hitShape = const_cast<AACubeShape*>(this);
hit = true;
}
}
}
}
}
return hit;
}

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

@ -25,7 +25,7 @@ public:
float getScale() const { return _scale; }
void setScale(float scale) { _scale = scale; }
bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
bool findRayIntersection(RayIntersectionInfo& intersection) const;
float getVolume() const { return _scale * _scale * _scale; }

136
libraries/shared/src/CapsuleShape.cpp
View file

@ -78,13 +78,135 @@ void CapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& en
updateBoundingRadius();
}
bool CapsuleShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
glm::vec3 capsuleStart, capsuleEnd;
getStartPoint(capsuleStart);
getEndPoint(capsuleEnd);
// NOTE: findRayCapsuleIntersection returns 'true' with distance = 0 when rayStart is inside capsule.
// TODO: implement the raycast to return inside surface intersection for the internal rayStart.
return findRayCapsuleIntersection(rayStart, rayDirection, capsuleStart, capsuleEnd, _radius, distance);
// helper
bool findRayIntersectionWithCap(const glm::vec3& sphereCenter, float sphereRadius,
const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) {
float r2 = sphereRadius * sphereRadius;
// compute closest approach (CA)
float a = glm::dot(sphereCenter - intersection._rayStart, intersection._rayDirection); // a = distance from ray-start to CA
float b2 = glm::distance2(sphereCenter, intersection._rayStart + a * intersection._rayDirection); // b2 = squared distance from sphere-center to CA
if (b2 > r2) {
// ray does not hit sphere
return false;
}
float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along intersection._rayDirection
float d2 = glm::distance2(intersection._rayStart, sphereCenter); // d2 = squared distance from sphere-center to ray-start
float distance = FLT_MAX;
if (a < 0.0f) {
// ray points away from sphere-center
if (d2 > r2) {
// ray starts outside sphere
return false;
}
// ray starts inside sphere
distance = c + a;
} else if (d2 > r2) {
// ray starts outside sphere
distance = a - c;
} else {
// ray starts inside sphere
distance = a + c;
}
if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) {
glm::vec3 sphereCenterToHitPoint = intersection._rayStart + distance * intersection._rayDirection - sphereCenter;
if (glm::dot(sphereCenterToHitPoint, sphereCenter - capsuleCenter) >= 0.0f) {
intersection._hitDistance = distance;
intersection._hitNormal = glm::normalize(sphereCenterToHitPoint);
return true;
}
}
return false;
}
bool CapsuleShape::findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const {
glm::vec3 capCenter;
getStartPoint(capCenter);
bool hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection);
getEndPoint(capCenter);
hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection) || hit;
if (hit) {
intersection._hitShape = const_cast<CapsuleShape*>(this);
}
return hit;
}
bool CapsuleShape::findRayIntersection(RayIntersectionInfo& intersection) const {
// ray is U, capsule is V
glm::vec3 axisV;
computeNormalizedAxis(axisV);
glm::vec3 centerV = getTranslation();
// first handle parallel case
float uDotV = glm::dot(axisV, intersection._rayDirection);
glm::vec3 UV = intersection._rayStart - centerV;
if (glm::abs(1.0f - glm::abs(uDotV)) < EPSILON) {
// line and cylinder are parallel
float distanceV = glm::dot(UV, intersection._rayDirection);
if (glm::length2(UV - distanceV * intersection._rayDirection) <= _radius * _radius) {
// ray is inside cylinder's radius and might intersect caps
return findRayIntersectionWithCaps(centerV, intersection);
}
return false;
}
// Given a line with point 'U' and normalized direction 'u' and
// a cylinder with axial point 'V', radius 'r', and normalized direction 'v'
// the intersection of the two is on the line at distance 't' from 'U'.
//
// Determining the values of t reduces to solving a quadratic equation: At^2 + Bt + C = 0
//
// where:
//
// UV = U-V
// w = u-(u.v)v
// Q = UV-(UV.v)v
//
// A = w^2
// B = 2(w.Q)
// C = Q^2 - r^2
glm::vec3 w = intersection._rayDirection - uDotV * axisV;
glm::vec3 Q = UV - glm::dot(UV, axisV) * axisV;
// we save a few multiplies by storing 2*A rather than just A
float A2 = 2.0f * glm::dot(w, w);
float B = 2.0f * glm::dot(w, Q);
// since C is only ever used once (in the determinant) we compute it inline
float determinant = B * B - 2.0f * A2 * (glm::dot(Q, Q) - _radius * _radius);
if (determinant < 0.0f) {
return false;
}
float hitLow = (-B - sqrtf(determinant)) / A2;
float hitHigh = -(hitLow + 2.0f * B / A2);
if (hitLow > hitHigh) {
// re-arrange so hitLow is always the smaller value
float temp = hitHigh;
hitHigh = hitLow;
hitLow = temp;
}
if (hitLow < 0.0f) {
if (hitHigh < 0.0f) {
// capsule is completely behind rayStart
return false;
}
hitLow = hitHigh;
}
glm::vec3 p = intersection._rayStart + hitLow * intersection._rayDirection;
float d = glm::dot(p - centerV, axisV);
if (glm::abs(d) <= getHalfHeight()) {
// we definitely hit the cylinder wall
intersection._hitDistance = hitLow;
intersection._hitNormal = glm::normalize(p - centerV - d * axisV);
intersection._hitShape = const_cast<CapsuleShape*>(this);
return true;
}
// ray still might hit the caps
return findRayIntersectionWithCaps(centerV, intersection);
}
// static

3
libraries/shared/src/CapsuleShape.h
View file

@ -47,11 +47,12 @@ public:
/// Sets the endpoints and updates center, rotation, and halfHeight to agree.
virtual void setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint);
bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
bool findRayIntersection(RayIntersectionInfo& intersection) const;
virtual float getVolume() const { return (PI * _radius * _radius) * (1.3333333333f * _radius + getHalfHeight()); }
protected:
bool findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const;
virtual void updateBoundingRadius() { _boundingRadius = _radius + getHalfHeight(); }
static glm::quat computeNewRotation(const glm::vec3& newAxis);

19
libraries/shared/src/PhysicsEntity.cpp
View file

@ -76,23 +76,8 @@ void PhysicsEntity::clearShapes() {
_shapes.clear();
}
bool PhysicsEntity::findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const {
int numShapes = _shapes.size();
float minDistance = FLT_MAX;
for (int j = 0; j < numShapes; ++j) {
const Shape* shape = _shapes[j];
float thisDistance = FLT_MAX;
if (shape && shape->findRayIntersection(origin, direction, thisDistance)) {
if (thisDistance < minDistance) {
minDistance = thisDistance;
}
}
}
if (minDistance < FLT_MAX) {
distance = minDistance;
return true;
}
return false;
bool PhysicsEntity::findRayIntersection(RayIntersectionInfo& intersection) const {
return ShapeCollider::findRayIntersection(_shapes, intersection);
}
bool PhysicsEntity::findCollisions(const QVector<const Shape*> shapes, CollisionList& collisions) {

3
libraries/shared/src/PhysicsEntity.h
View file

@ -19,6 +19,7 @@
#include <glm/gtc/quaternion.hpp>
#include "CollisionInfo.h"
#include "RayIntersectionInfo.h"
class Shape;
class PhysicsSimulation;
@ -52,7 +53,7 @@ public:
PhysicsSimulation* getSimulation() const { return _simulation; }
bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
bool findRayIntersection(RayIntersectionInfo& intersection) const;
bool findCollisions(const QVector<const Shape*> shapes, CollisionList& collisions);
bool findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius, CollisionList& collisions);
bool findPlaneCollisions(const glm::vec4& plane, CollisionList& collisions);

50
libraries/shared/src/PhysicsSimulation.cpp
View file

@ -207,9 +207,6 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) {
++_frameCount;
if (!_ragdoll) {
return;
}
quint64 now = usecTimestampNow();
quint64 startTime = now;
quint64 expiry = startTime + maxUsec;
@ -219,7 +216,9 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
int numDolls = _otherRagdolls.size();
{
PerformanceTimer perfTimer("enforce");
_ragdoll->enforceConstraints();
if (_ragdoll) {
_ragdoll->enforceConstraints();
}
for (int i = 0; i < numDolls; ++i) {
_otherRagdolls[i]->enforceConstraints();
}
@ -235,7 +234,9 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
{ // enforce constraints
PerformanceTimer perfTimer("enforce");
error = _ragdoll->enforceConstraints();
if (_ragdoll) {
error = _ragdoll->enforceConstraints();
}
for (int i = 0; i < numDolls; ++i) {
error = glm::max(error, _otherRagdolls[i]->enforceConstraints());
}
@ -246,9 +247,12 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
now = usecTimestampNow();
} while (_collisions.size() != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry));
// the collisions may have moved the main ragdoll from the simulation center
// so we remove this offset (potentially storing it as movement of the Ragdoll owner)
_ragdoll->removeRootOffset(collidedWithOtherRagdoll);
if (_ragdoll) {
// This is why _ragdoll is special and is not in the list of other ragdolls:
// The collisions may have moved the main ragdoll from the simulation center
// so we remove this offset (potentially storing it as movement of the Ragdoll owner)
_ragdoll->removeRootOffset(collidedWithOtherRagdoll);
}
// also remove any offsets from the other ragdolls
for (int i = 0; i < numDolls; ++i) {
@ -257,13 +261,41 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
pruneContacts();
}
bool PhysicsSimulation::findFloorRayIntersection(RayIntersectionInfo& intersection) const {
// only casts against otherEntities
bool hit = false;
int numEntities = _otherEntities.size();
for (int i = 0; i < numEntities; ++i) {
const QVector<Shape*> otherShapes = _otherEntities.at(i)->getShapes();
if (ShapeCollider::findRayIntersection(otherShapes, intersection)) {
hit = true;
}
}
return hit;
}
bool PhysicsSimulation::getShapeCollisions(const Shape* shape, CollisionList& collisions) const {
bool hit = false;
int numEntities = _otherEntities.size();
for (int i = 0; i < numEntities; ++i) {
const QVector<Shape*> otherShapes = _otherEntities.at(i)->getShapes();
if (ShapeCollider::collideShapeWithShapes(shape, otherShapes, 0, collisions)) {
hit = true;
}
}
return hit;
}
void PhysicsSimulation::integrate(float deltaTime) {
PerformanceTimer perfTimer("integrate");
int numEntities = _otherEntities.size();
for (int i = 0; i < numEntities; ++i) {
_otherEntities[i]->stepForward(deltaTime);
}
_ragdoll->stepForward(deltaTime);
if (_ragdoll) {
_ragdoll->stepForward(deltaTime);
}
int numDolls = _otherRagdolls.size();
for (int i = 0; i < numDolls; ++i) {
_otherRagdolls[i]->stepForward(deltaTime);

13
libraries/shared/src/PhysicsSimulation.h
View file

@ -9,8 +9,8 @@
// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//
#ifndef hifi_PhysicsSimulation
#define hifi_PhysicsSimulation
#ifndef hifi_PhysicsSimulation_h
#define hifi_PhysicsSimulation_h
#include <QtGlobal>
#include <QMap>
@ -18,6 +18,7 @@
#include "CollisionInfo.h"
#include "ContactPoint.h"
#include "RayIntersectionInfo.h"
class PhysicsEntity;
class Ragdoll;
@ -54,6 +55,12 @@ public:
/// \return distance of largest movement
void stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec);
/// \param intersection collision info about ray hit
/// \return true if ray hits any shape that doesn't belong to the main ragdoll/entity
bool findFloorRayIntersection(RayIntersectionInfo& hit) const;
bool getShapeCollisions(const Shape* shape, CollisionList& collisions) const;
protected:
void integrate(float deltaTime);
@ -80,4 +87,4 @@ private:
QMap<quint64, ContactPoint> _contacts;
};
#endif // hifi_PhysicsSimulation
#endif // hifi_PhysicsSimulation_h

33
libraries/shared/src/PlaneShape.cpp
View file

@ -11,6 +11,7 @@
#include "PlaneShape.h"
#include "SharedUtil.h"
#include "GLMHelpers.h"
const glm::vec3 UNROTATED_NORMAL(0.0f, 1.0f, 0.0f);
@ -34,22 +35,42 @@ glm::vec3 PlaneShape::getNormal() const {
return _rotation * UNROTATED_NORMAL;
}
void PlaneShape::setNormal(const glm::vec3& direction) {
glm::vec3 oldTranslation = _translation;
_rotation = rotationBetween(UNROTATED_NORMAL, direction);
glm::vec3 normal = getNormal();
_translation = glm::dot(oldTranslation, normal) * normal;
}
void PlaneShape::setPoint(const glm::vec3& point) {
glm::vec3 normal = getNormal();
_translation = glm::dot(point, normal) * normal;
}
glm::vec4 PlaneShape::getCoefficients() const {
glm::vec3 normal = _rotation * UNROTATED_NORMAL;
return glm::vec4(normal.x, normal.y, normal.z, -glm::dot(normal, _translation));
}
bool PlaneShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
bool PlaneShape::findRayIntersection(RayIntersectionInfo& intersection) const {
glm::vec3 n = getNormal();
float denominator = glm::dot(n, rayDirection);
float denominator = glm::dot(n, intersection._rayDirection);
if (fabsf(denominator) < EPSILON) {
// line is parallel to plane
return glm::dot(_translation - rayStart, n) < EPSILON;
if (glm::dot(_translation - intersection._rayStart, n) < EPSILON) {
// ray starts on the plane
intersection._hitDistance = 0.0f;
intersection._hitNormal = n;
intersection._hitShape = const_cast<PlaneShape*>(this);
return true;
}
} else {
float d = glm::dot(_translation - rayStart, n) / denominator;
if (d > 0.0f) {
float d = glm::dot(_translation - intersection._rayStart, n) / denominator;
if (d > 0.0f && d < intersection._rayLength && d < intersection._hitDistance) {
// ray points toward plane
distance = d;
intersection._hitDistance = d;
intersection._hitNormal = n;
intersection._hitShape = const_cast<PlaneShape*>(this);
return true;
}
}

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

@ -21,7 +21,10 @@ public:
glm::vec3 getNormal() const;
glm::vec4 getCoefficients() const;
bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
void setNormal(const glm::vec3& normal);
void setPoint(const glm::vec3& point);
bool findRayIntersection(RayIntersectionInfo& intersection) const;
};
#endif // hifi_PlaneShape_h

37
libraries/shared/src/RayIntersectionInfo.h Normal file
View file

@ -0,0 +1,37 @@
//
// RayIntersectionInfo.h
// interface/src/avatar
//
// Created by Andrew Meadows 2014.09.09
// Copyright 2014 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_RayIntersectionInfo_h
#define hifi_RayIntersectionInfo_h
#include <glm/glm.hpp>
class Shape;
class RayIntersectionInfo {
public:
RayIntersectionInfo() : _rayStart(0.0f), _rayDirection(1.0f, 0.0f, 0.0f), _rayLength(FLT_MAX),
_hitDistance(FLT_MAX), _hitNormal(1.0f, 0.0f, 0.0f), _hitShape(NULL) { }
glm::vec3 getIntersectionPoint() const { return _rayStart + _hitDistance * _rayDirection; }
// input
glm::vec3 _rayStart;
glm::vec3 _rayDirection;
float _rayLength;
// output
float _hitDistance;
glm::vec3 _hitNormal;
Shape* _hitShape;
};
#endif // hifi_RayIntersectionInfo_h

4
libraries/shared/src/Shape.h
View file

@ -17,6 +17,8 @@
#include <QtGlobal>
#include <QVector>
#include "RayIntersectionInfo.h"
class PhysicsEntity;
class VerletPoint;
@ -59,7 +61,7 @@ public:
virtual void setMass(float mass) { _mass = mass; }
virtual float getMass() const { return _mass; }
virtual bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const = 0;
virtual bool findRayIntersection(RayIntersectionInfo& intersection) const = 0;
/// \param penetration of collision
/// \param contactPoint of collision

16
libraries/shared/src/ShapeCollider.cpp
View file

@ -1087,24 +1087,18 @@ bool capsuleVsAACubeLegacy(const CapsuleShape* capsuleA, const glm::vec3& cubeCe
return sphereVsAACubeLegacy(nearestApproach, capsuleA->getRadius(), cubeCenter, cubeSide, collisions);
}
bool findRayIntersectionWithShapes(const QVector<Shape*> shapes, const glm::vec3& rayStart, const glm::vec3& rayDirection, float& minDistance) {
float hitDistance = FLT_MAX;
bool findRayIntersection(const QVector<Shape*>& shapes, RayIntersectionInfo& intersection) {
int numShapes = shapes.size();
bool hit = false;
for (int i = 0; i < numShapes; ++i) {
Shape* shape = shapes.at(i);
if (shape) {
float distance;
if (shape->findRayIntersection(rayStart, rayDirection, distance)) {
if (distance < hitDistance) {
hitDistance = distance;
}
if (shape->findRayIntersection(intersection)) {
hit = true;
}
}
}
if (hitDistance < FLT_MAX) {
minDistance = hitDistance;
}
return false;
return hit;
}
} // namespace ShapeCollider

7
libraries/shared/src/ShapeCollider.h
View file

@ -15,6 +15,7 @@
#include <QVector>
#include "CollisionInfo.h"
#include "RayIntersectionInfo.h"
#include "SharedUtil.h"
class Shape;
@ -145,11 +146,9 @@ namespace ShapeCollider {
bool capsuleVsAACubeLegacy(const CapsuleShape* capsuleA, const glm::vec3& cubeCenter, float cubeSide, CollisionList& collisions);
/// \param shapes list of pointers to shapes (shape pointers may be NULL)
/// \param startPoint beginning of ray
/// \param direction direction of ray
/// \param minDistance[out] shortest distance to intersection of ray with a shapes
/// \param intersection[out] struct with info about Ray and hit
/// \return true if ray hits any shape in shapes
bool findRayIntersectionWithShapes(const QVector<Shape*> shapes, const glm::vec3& startPoint, const glm::vec3& direction, float& minDistance);
bool findRayIntersection(const QVector<Shape*>& shapes, RayIntersectionInfo& intersection);
} // namespace ShapeCollider

19
libraries/shared/src/SphereShape.cpp
View file

@ -13,18 +13,19 @@
#include "SphereShape.h"
bool SphereShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
bool SphereShape::findRayIntersection(RayIntersectionInfo& intersection) const {
float r2 = _boundingRadius * _boundingRadius;
// compute closest approach (CA)
float a = glm::dot(_translation - rayStart, rayDirection); // a = distance from ray-start to CA
float b2 = glm::distance2(_translation, rayStart + a * rayDirection); // b2 = squared distance from sphere-center to CA
float a = glm::dot(_translation - intersection._rayStart, intersection._rayDirection); // a = distance from ray-start to CA
float b2 = glm::distance2(_translation, intersection._rayStart + a * intersection._rayDirection); // b2 = squared distance from sphere-center to CA
if (b2 > r2) {
// ray does not hit sphere
return false;
}
float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along rayDirection
float d2 = glm::distance2(rayStart, _translation); // d2 = squared distance from sphere-center to ray-start
float c = sqrtf(r2 - b2); // c = distance from CA to sphere surface along intersection._rayDirection
float d2 = glm::distance2(intersection._rayStart, _translation); // d2 = squared distance from sphere-center to ray-start
float distance = FLT_MAX;
if (a < 0.0f) {
// ray points away from sphere-center
if (d2 > r2) {
@ -40,5 +41,11 @@ bool SphereShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3
// ray starts inside sphere
distance = a + c;
}
return true;
if (distance > 0.0f && distance < intersection._rayLength && distance < intersection._hitDistance) {
intersection._hitDistance = distance;
intersection._hitNormal = glm::normalize(intersection._rayStart + distance * intersection._rayDirection - _translation);
intersection._hitShape = const_cast<SphereShape*>(this);
return true;
}
return false;
}

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

@ -34,7 +34,7 @@ public:
void setRadius(float radius) { _boundingRadius = radius; }
bool findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const;
bool findRayIntersection(RayIntersectionInfo& intersection) const;
float getVolume() const { return 1.3333333333f * PI * _boundingRadius * _boundingRadius * _boundingRadius; }
};

613
tests/physics/src/ShapeColliderTests.cpp
View file

@ -1803,40 +1803,44 @@ void ShapeColliderTests::capsuleTouchesAACube() {
void ShapeColliderTests::rayHitsSphere() {
float startDistance = 3.0f;
glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
glm::vec3 rayDirection(1.0f, 0.0f, 0.0f);
float radius = 1.0f;
glm::vec3 center(0.0f);
SphereShape sphere(radius, center);
// very simple ray along xAxis
{
float distance = FLT_MAX;
if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
RayIntersectionInfo intersection;
intersection._rayStart = -startDistance * xAxis;
intersection._rayDirection = xAxis;
if (!sphere.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl;
}
float expectedDistance = startDistance - radius;
float relativeError = fabsf(distance - expectedDistance) / startDistance;
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl;
}
if (intersection._hitShape != &sphere) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at sphere"
<< std::endl;
}
}
// ray along a diagonal axis
{
rayStart = glm::vec3(startDistance, startDistance, 0.0f);
rayDirection = - glm::normalize(rayStart);
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(startDistance, startDistance, 0.0f);
intersection._rayDirection = - glm::normalize(intersection._rayStart);
float distance = FLT_MAX;
if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
if (!sphere.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl;
}
float expectedDistance = SQUARE_ROOT_OF_2 * startDistance - radius;
float relativeError = fabsf(distance - expectedDistance) / startDistance;
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = " << relativeError << std::endl;
}
@ -1851,22 +1855,22 @@ void ShapeColliderTests::rayHitsSphere() {
glm::quat rotation = glm::angleAxis(0.987654321f, axis);
glm::vec3 translation(35.7f, 2.46f, -1.97f);
glm::vec3 unrotatedRayDirection(-1.0f, 0.0f, 0.0f);
glm::vec3 untransformedRayStart(startDistance, 0.0f, 0.0f);
glm::vec3 unrotatedRayDirection = -xAxis;
glm::vec3 untransformedRayStart = startDistance * xAxis;
rayStart = rotation * (untransformedRayStart + translation);
rayDirection = rotation * unrotatedRayDirection;
RayIntersectionInfo intersection;
intersection._rayStart = rotation * (untransformedRayStart + translation);
intersection._rayDirection = rotation * unrotatedRayDirection;
sphere.setRadius(radius);
sphere.setTranslation(rotation * translation);
float distance = FLT_MAX;
if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
if (!sphere.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should intersect sphere" << std::endl;
}
float expectedDistance = startDistance - radius;
float relativeError = fabsf(distance - expectedDistance) / startDistance;
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray sphere intersection distance error = "
<< relativeError << std::endl;
@ -1879,31 +1883,40 @@ void ShapeColliderTests::rayBarelyHitsSphere() {
glm::vec3 center(0.0f);
float delta = 2.0f * EPSILON;
float startDistance = 3.0f;
glm::vec3 rayStart(-startDistance, radius - delta, 0.0f);
glm::vec3 rayDirection(1.0f, 0.0f, 0.0f);
SphereShape sphere(radius, center);
float startDistance = 3.0f;
// very simple ray along xAxis
float distance = FLT_MAX;
if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
{
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(-startDistance, radius - delta, 0.0f);
intersection._rayDirection = xAxis;
// very simple ray along xAxis
if (!sphere.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
}
if (intersection._hitShape != &sphere) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at sphere"
<< std::endl;
}
}
// translate and rotate the whole system...
glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
glm::quat rotation = glm::angleAxis(0.987654321f, axis);
glm::vec3 translation(35.7f, 2.46f, -1.97f);
rayStart = rotation * (rayStart + translation);
rayDirection = rotation * rayDirection;
sphere.setTranslation(rotation * translation);
// ...and test again
distance = FLT_MAX;
if (!sphere.findRayIntersection(rayStart, rayDirection, distance)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
{
// translate and rotate the whole system...
glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
glm::quat rotation = glm::angleAxis(0.987654321f, axis);
glm::vec3 translation(35.7f, 0.46f, -1.97f);
RayIntersectionInfo intersection;
intersection._rayStart = rotation * (intersection._rayStart + translation);
intersection._rayDirection = rotation * intersection._rayDirection;
sphere.setTranslation(rotation * translation);
// ...and test again
if (!sphere.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely hit sphere" << std::endl;
}
}
}
@ -1914,39 +1927,47 @@ void ShapeColliderTests::rayBarelyMissesSphere() {
glm::vec3 center(0.0f);
float delta = 2.0f * EPSILON;
float startDistance = 3.0f;
glm::vec3 rayStart(-startDistance, radius + delta, 0.0f);
glm::vec3 rayDirection(1.0f, 0.0f, 0.0f);
SphereShape sphere(radius, center);
float startDistance = 3.0f;
// very simple ray along xAxis
float distance = FLT_MAX;
if (sphere.findRayIntersection(rayStart, rayDirection, distance)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
{
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(-startDistance, radius + delta, 0.0f);
intersection._rayDirection = xAxis;
// very simple ray along xAxis
if (sphere.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
}
if (intersection._hitDistance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
}
// translate and rotate the whole system...
glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
glm::quat rotation = glm::angleAxis(0.987654321f, axis);
glm::vec3 translation(35.7f, 2.46f, -1.97f);
{
// translate and rotate the whole system...
float angle = 0.987654321f;
glm::vec3 axis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
glm::quat rotation = glm::angleAxis(angle, axis);
glm::vec3 translation(35.7f, 2.46f, -1.97f);
rayStart = rotation * (rayStart + translation);
rayDirection = rotation * rayDirection;
sphere.setTranslation(rotation * translation);
// ...and test again
distance = FLT_MAX;
if (sphere.findRayIntersection(rayStart, rayDirection, distance)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
}
if (distance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
RayIntersectionInfo intersection;
intersection._rayStart = rotation * (glm::vec3(-startDistance, radius + delta, 0.0f) + translation);
intersection._rayDirection = rotation * xAxis;
sphere.setTranslation(rotation * translation);
// ...and test again
if (sphere.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should just barely miss sphere" << std::endl;
}
if (intersection._hitDistance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
if (intersection._hitShape != NULL) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should be NULL" << std::endl;
}
}
}
@ -1957,85 +1978,99 @@ void ShapeColliderTests::rayHitsCapsule() {
glm::vec3 center(0.0f);
CapsuleShape capsule(radius, halfHeight);
{ // simple test along xAxis
// toward capsule center
glm::vec3 rayStart(startDistance, 0.0f, 0.0f);
glm::vec3 rayDirection(-1.0f, 0.0f, 0.0f);
float distance = FLT_MAX;
if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
// simple tests along xAxis
{ // toward capsule center
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(startDistance, 0.0f, 0.0f);
intersection._rayDirection = - xAxis;
if (!capsule.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
}
float expectedDistance = startDistance - radius;
float relativeError = fabsf(distance - expectedDistance) / startDistance;
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
if (intersection._hitShape != &capsule) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at capsule"
<< std::endl;
}
}
// toward top of cylindrical wall
rayStart.y = halfHeight;
distance = FLT_MAX;
if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
{ // toward top of cylindrical wall
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(startDistance, halfHeight, 0.0f);
intersection._rayDirection = - xAxis;
if (!capsule.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
}
relativeError = fabsf(distance - expectedDistance) / startDistance;
float expectedDistance = startDistance - radius;
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
}
// toward top cap
float delta = 2.0f * EPSILON;
rayStart.y = halfHeight + delta;
distance = FLT_MAX;
if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
float delta = 2.0f * EPSILON;
{ // toward top cap
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(startDistance, halfHeight + delta, 0.0f);
intersection._rayDirection = - xAxis;
if (!capsule.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
}
relativeError = fabsf(distance - expectedDistance) / startDistance;
float expectedDistance = startDistance - radius;
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
}
const float EDGE_CASE_SLOP_FACTOR = 20.0f;
// toward tip of top cap
rayStart.y = halfHeight + radius - delta;
distance = FLT_MAX;
if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
const float EDGE_CASE_SLOP_FACTOR = 20.0f;
{ // toward tip of top cap
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(startDistance, halfHeight + radius - delta, 0.0f);
intersection._rayDirection = - xAxis;
if (!capsule.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
}
expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine
relativeError = fabsf(distance - expectedDistance) / startDistance;
float expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
// for edge cases we allow a LOT of error
if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
}
// toward tip of bottom cap
rayStart.y = - halfHeight - radius + delta;
distance = FLT_MAX;
if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
{ // toward tip of bottom cap
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(startDistance, - halfHeight - radius + delta, 0.0f);
intersection._rayDirection = - xAxis;
if (!capsule.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
}
expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine
relativeError = fabsf(distance - expectedDistance) / startDistance;
float expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
// for edge cases we allow a LOT of error
if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
<< relativeError << std::endl;
}
}
// toward edge of capsule cylindrical face
rayStart.y = 0.0f;
rayStart.z = radius - delta;
distance = FLT_MAX;
if (!capsule.findRayIntersection(rayStart, rayDirection, distance)) {
{ // toward edge of capsule cylindrical face
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(startDistance, 0.0f, radius - delta);
intersection._rayDirection = - xAxis;
if (!capsule.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit capsule" << std::endl;
}
expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine
relativeError = fabsf(distance - expectedDistance) / startDistance;
float expectedDistance = startDistance - radius * sqrtf(2.0f * delta); // using small angle approximation of cosine
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / startDistance;
// for edge cases we allow a LOT of error
if (relativeError > EDGE_CASE_SLOP_FACTOR * EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray capsule intersection distance error = "
@ -2055,43 +2090,47 @@ void ShapeColliderTests::rayMissesCapsule() {
{ // simple test along xAxis
// toward capsule center
glm::vec3 rayStart(startDistance, 0.0f, 0.0f);
glm::vec3 rayDirection(-1.0f, 0.0f, 0.0f);
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(startDistance, 0.0f, 0.0f);
intersection._rayDirection = -xAxis;
float delta = 2.0f * EPSILON;
// over top cap
rayStart.y = halfHeight + radius + delta;
float distance = FLT_MAX;
if (capsule.findRayIntersection(rayStart, rayDirection, distance)) {
intersection._rayStart.y = halfHeight + radius + delta;
intersection._hitDistance = FLT_MAX;
if (capsule.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
}
if (distance != FLT_MAX) {
if (intersection._hitDistance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
// below bottom cap
rayStart.y = - halfHeight - radius - delta;
distance = FLT_MAX;
if (capsule.findRayIntersection(rayStart, rayDirection, distance)) {
intersection._rayStart.y = - halfHeight - radius - delta;
intersection._hitDistance = FLT_MAX;
if (capsule.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
}
if (distance != FLT_MAX) {
if (intersection._hitDistance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
// past edge of capsule cylindrical face
rayStart.y = 0.0f;
rayStart.z = radius + delta;
distance = FLT_MAX;
if (capsule.findRayIntersection(rayStart, rayDirection, distance)) {
intersection._rayStart.y = 0.0f;
intersection._rayStart.z = radius + delta;
intersection._hitDistance = FLT_MAX;
if (capsule.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss capsule" << std::endl;
}
if (distance != FLT_MAX) {
if (intersection._hitDistance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
if (intersection._hitShape != NULL) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should be NULL" << std::endl;
}
}
// TODO: test at steep angles near edge
}
@ -2101,45 +2140,53 @@ void ShapeColliderTests::rayHitsPlane() {
float planeDistanceFromOrigin = 3.579f;
glm::vec3 planePosition(0.0f, planeDistanceFromOrigin, 0.0f);
PlaneShape plane;
plane.setTranslation(planePosition);
plane.setPoint(planePosition);
plane.setNormal(yAxis);
// make a simple ray
float startDistance = 1.234f;
glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
glm::vec3 rayDirection = glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f));
float distance = FLT_MAX;
if (!plane.findRayIntersection(rayStart, rayDirection, distance)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
{
RayIntersectionInfo intersection;
intersection._rayStart = -startDistance * xAxis;
intersection._rayDirection = glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f));
if (!plane.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
}
float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / planeDistanceFromOrigin;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = "
<< relativeError << std::endl;
}
if (intersection._hitShape != &plane) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at plane"
<< std::endl;
}
}
float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
float relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = "
<< relativeError << std::endl;
}
// rotate the whole system and try again
float angle = 37.8f;
glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) );
glm::quat rotation = glm::angleAxis(angle, axis);
plane.setTranslation(rotation * planePosition);
plane.setRotation(rotation);
rayStart = rotation * rayStart;
rayDirection = rotation * rayDirection;
distance = FLT_MAX;
if (!plane.findRayIntersection(rayStart, rayDirection, distance)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
}
expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
relativeError = fabsf(distance - expectedDistance) / planeDistanceFromOrigin;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = "
<< relativeError << std::endl;
{ // rotate the whole system and try again
float angle = 37.8f;
glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) );
glm::quat rotation = glm::angleAxis(angle, axis);
plane.setNormal(rotation * yAxis);
plane.setPoint(rotation * planePosition);
RayIntersectionInfo intersection;
intersection._rayStart = rotation * (-startDistance * xAxis);
intersection._rayDirection = rotation * glm::normalize(glm::vec3(1.0f, 1.0f, 1.0f));
if (!plane.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit plane" << std::endl;
}
float expectedDistance = SQUARE_ROOT_OF_3 * planeDistanceFromOrigin;
float relativeError = fabsf(intersection._hitDistance - expectedDistance) / planeDistanceFromOrigin;
if (relativeError > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray plane intersection distance error = "
<< relativeError << std::endl;
}
}
}
@ -2152,14 +2199,14 @@ void ShapeColliderTests::rayMissesPlane() {
{ // parallel rays should miss
float startDistance = 1.234f;
glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, 0.0f, -1.0f));
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(-startDistance, 0.0f, 0.0f);
intersection._rayDirection = glm::normalize(glm::vec3(-1.0f, 0.0f, -1.0f));
float distance = FLT_MAX;
if (plane.findRayIntersection(rayStart, rayDirection, distance)) {
if (plane.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
}
if (distance != FLT_MAX) {
if (intersection._hitDistance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
@ -2171,29 +2218,35 @@ void ShapeColliderTests::rayMissesPlane() {
plane.setTranslation(rotation * planePosition);
plane.setRotation(rotation);
rayStart = rotation * rayStart;
rayDirection = rotation * rayDirection;
distance = FLT_MAX;
if (plane.findRayIntersection(rayStart, rayDirection, distance)) {
intersection._rayStart = rotation * intersection._rayStart;
intersection._rayDirection = rotation * intersection._rayDirection;
intersection._hitDistance = FLT_MAX;
if (plane.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
}
if (distance != FLT_MAX) {
if (intersection._hitDistance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
if (intersection._hitShape != NULL) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should be NULL" << std::endl;
}
}
{ // make a simple ray that points away from plane
float startDistance = 1.234f;
glm::vec3 rayStart(-startDistance, 0.0f, 0.0f);
glm::vec3 rayDirection = glm::normalize(glm::vec3(-1.0f, -1.0f, -1.0f));
RayIntersectionInfo intersection;
intersection._rayStart = glm::vec3(-startDistance, 0.0f, 0.0f);
intersection._rayDirection = glm::normalize(glm::vec3(-1.0f, -1.0f, -1.0f));
intersection._hitDistance = FLT_MAX;
float distance = FLT_MAX;
if (plane.findRayIntersection(rayStart, rayDirection, distance)) {
if (plane.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
}
if (distance != FLT_MAX) {
if (intersection._hitDistance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
@ -2205,20 +2258,225 @@ void ShapeColliderTests::rayMissesPlane() {
plane.setTranslation(rotation * planePosition);
plane.setRotation(rotation);
rayStart = rotation * rayStart;
rayDirection = rotation * rayDirection;
intersection._rayStart = rotation * intersection._rayStart;
intersection._rayDirection = rotation * intersection._rayDirection;
intersection._hitDistance = FLT_MAX;
distance = FLT_MAX;
if (plane.findRayIntersection(rayStart, rayDirection, distance)) {
if (plane.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should miss plane" << std::endl;
}
if (distance != FLT_MAX) {
if (intersection._hitDistance != FLT_MAX) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: distance should be unchanged after intersection miss"
<< std::endl;
}
}
}
void ShapeColliderTests::rayHitsAACube() {
glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f);
float cubeSide = 2.127f;
AACubeShape cube(cubeSide, cubeCenter);
float rayOffset = 3.796f;
glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis};
int numDirections = 3;
int numRayCasts = 5;
for (int i = 0; i < numDirections; ++i) {
for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
glm::vec3 faceNormal = sign * faceNormals[i];
glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
// pick a random point somewhere above the face
glm::vec3 rayStart = cubeCenter +
(cubeSide + rayOffset) * faceNormal +
(cubeSide * (randFloat() - 0.5f)) * secondNormal +
(cubeSide * (randFloat() - 0.5f)) * thirdNormal;
// cast multiple rays toward the face
for (int j = 0; j < numRayCasts; ++j) {
// pick a random point on the face
glm::vec3 facePoint = cubeCenter +
0.5f * cubeSide * faceNormal +
(cubeSide * (randFloat() - 0.5f)) * secondNormal +
(cubeSide * (randFloat() - 0.5f)) * thirdNormal;
// construct a ray from first point through second point
RayIntersectionInfo intersection;
intersection._rayStart = rayStart;
intersection._rayDirection = glm::normalize(facePoint - rayStart);
intersection._rayLength = 1.0001f * glm::distance(rayStart, facePoint);
// cast the ray
bool hit = cube.findRayIntersection(intersection);
// validate
if (!hit) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should hit cube face" << std::endl;
break;
}
if (glm::abs(1.0f - glm::dot(faceNormal, intersection._hitNormal)) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: ray should hit cube face with normal " << faceNormal
<< " but found different normal " << intersection._hitNormal << std::endl;
}
if (glm::distance(facePoint, intersection.getIntersectionPoint()) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: ray should hit cube face at " << facePoint
<< " but actually hit at " << intersection.getIntersectionPoint()
<< std::endl;
}
if (intersection._hitShape != &cube) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray intersection._hitShape should point at cube"
<< std::endl;
}
}
}
}
}
void ShapeColliderTests::rayMissesAACube() {
//glm::vec3 cubeCenter(1.23f, 4.56f, 7.89f);
//float cubeSide = 2.127f;
glm::vec3 cubeCenter(0.0f);
float cubeSide = 2.f;
AACubeShape cube(cubeSide, cubeCenter);
float rayOffset = 3.796f;
glm::vec3 faceNormals[] = {xAxis, yAxis, zAxis};
int numDirections = 3;
int numRayCasts = 5;
const float SOME_SMALL_NUMBER = 0.0001f;
{ // ray misses cube for being too short
for (int i = 0; i < numDirections; ++i) {
for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
glm::vec3 faceNormal = sign * faceNormals[i];
glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
// pick a random point somewhere above the face
glm::vec3 rayStart = cubeCenter +
(cubeSide + rayOffset) * faceNormal +
(cubeSide * (randFloat() - 0.5f)) * secondNormal +
(cubeSide * (randFloat() - 0.5f)) * thirdNormal;
// cast multiple rays toward the face
for (int j = 0; j < numRayCasts; ++j) {
// pick a random point on the face
glm::vec3 facePoint = cubeCenter +
0.5f * cubeSide * faceNormal +
(cubeSide * (randFloat() - 0.5f)) * secondNormal +
(cubeSide * (randFloat() - 0.5f)) * thirdNormal;
// construct a ray from first point to almost second point
RayIntersectionInfo intersection;
intersection._rayStart = rayStart;
intersection._rayDirection = glm::normalize(facePoint - rayStart);
intersection._rayLength = (1.0f - SOME_SMALL_NUMBER) * glm::distance(rayStart, facePoint);
// cast the ray
if (cube.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should NOT hit cube face "
<< faceNormal << std::endl;
}
}
}
}
}
{ // long ray misses cube
for (int i = 0; i < numDirections; ++i) {
for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
glm::vec3 faceNormal = sign * faceNormals[i];
glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
// pick a random point somewhere above the face
glm::vec3 rayStart = cubeCenter +
(cubeSide + rayOffset) * faceNormal +
(cubeSide * (randFloat() - 0.5f)) * secondNormal +
(cubeSide * (randFloat() - 0.5f)) * thirdNormal;
// cast multiple rays that miss the face
for (int j = 0; j < numRayCasts; ++j) {
// pick a random point just outside of face
float inside = (cubeSide * (randFloat() - 0.5f));
float outside = 0.5f * cubeSide + SOME_SMALL_NUMBER * randFloat();
if (randFloat() - 0.5f < 0.0f) {
outside *= -1.0f;
}
glm::vec3 sidePoint = cubeCenter + 0.5f * cubeSide * faceNormal;
if (randFloat() - 0.5f < 0.0f) {
sidePoint += outside * secondNormal + inside * thirdNormal;
} else {
sidePoint += inside * secondNormal + outside * thirdNormal;
}
// construct a ray from first point through second point
RayIntersectionInfo intersection;
intersection._rayStart = rayStart;
intersection._rayDirection = glm::normalize(sidePoint - rayStart);
// cast the ray
if (cube.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should NOT hit cube face "
<< faceNormal << std::endl;
}
}
}
}
}
{ // ray parallel to face barely misses cube
for (int i = 0; i < numDirections; ++i) {
for (float sign = -1.0f; sign < 2.0f; sign += 2.0f) {
glm::vec3 faceNormal = sign * faceNormals[i];
glm::vec3 secondNormal = faceNormals[(i + 1) % numDirections];
glm::vec3 thirdNormal = faceNormals[(i + 2) % numDirections];
// cast multiple rays that miss the face
for (int j = 0; j < numRayCasts; ++j) {
// rayStart is above the face
glm::vec3 rayStart = cubeCenter + (0.5f + SOME_SMALL_NUMBER) * cubeSide * faceNormal;
// move rayStart to some random edge and choose the ray direction to point across the face
float inside = (cubeSide * (randFloat() - 0.5f));
float outside = 0.5f * cubeSide + SOME_SMALL_NUMBER * randFloat();
if (randFloat() - 0.5f < 0.0f) {
outside *= -1.0f;
}
glm::vec3 rayDirection = secondNormal;
if (randFloat() - 0.5f < 0.0f) {
rayStart += outside * secondNormal + inside * thirdNormal;
} else {
rayStart += inside * secondNormal + outside * thirdNormal;
rayDirection = thirdNormal;
}
if (outside > 0.0f) {
rayDirection *= -1.0f;
}
// construct a ray from first point through second point
RayIntersectionInfo intersection;
intersection._rayStart = rayStart;
intersection._rayDirection = rayDirection;
// cast the ray
if (cube.findRayIntersection(intersection)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: ray should NOT hit cube face "
<< faceNormal << std::endl;
}
}
}
}
}
}
void ShapeColliderTests::measureTimeOfCollisionDispatch() {
/* KEEP for future manual testing
// create two non-colliding spheres
@ -2278,4 +2536,7 @@ void ShapeColliderTests::runAllTests() {
rayMissesCapsule();
rayHitsPlane();
rayMissesPlane();
rayHitsAACube();
rayMissesAACube();
}

2
tests/physics/src/ShapeColliderTests.h
View file

@ -38,6 +38,8 @@ namespace ShapeColliderTests {
void rayMissesCapsule();
void rayHitsPlane();
void rayMissesPlane();
void rayHitsAACube();
void rayMissesAACube();
void measureTimeOfCollisionDispatch();