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

Merge pull request #4423 from AndrewMeadows/isentropic

Browse source 
remove old ragdoll simulation and avatar interactions
This commit is contained in:
Seth Alves 2015-03-11 15:18:15 -07:00
commit 48aca5c46f
38 changed files with 14 additions and 3548 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

9
interface/src/Menu.cpp
View file

@ -205,7 +205,6 @@ Menu::Menu() {
Qt::CTRL | Qt::SHIFT | Qt::Key_K, true, avatar, SLOT(updateMotionBehavior()));
addCheckableActionToQMenuAndActionHash(avatarMenu, MenuOption::ScriptedMotorControl, 0, true,
avatar, SLOT(updateMotionBehavior()));
addCheckableActionToQMenuAndActionHash(avatarMenu, MenuOption::ChatCircling, 0, false);
addCheckableActionToQMenuAndActionHash(avatarMenu, MenuOption::NamesAboveHeads, 0, true);
addCheckableActionToQMenuAndActionHash(avatarMenu, MenuOption::GlowWhenSpeaking, 0, true);
addCheckableActionToQMenuAndActionHash(avatarMenu, MenuOption::BlueSpeechSphere, 0, true);
@ -216,14 +215,6 @@ Menu::Menu() {
addCheckableActionToQMenuAndActionHash(avatarMenu, MenuOption::ShiftHipsForIdleAnimations, 0, false,
avatar, SLOT(updateMotionBehavior()));
QMenu* collisionsMenu = avatarMenu->addMenu("Collide With");
addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideAsRagdoll, 0, false,
avatar, SLOT(onToggleRagdoll()));
addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideWithAvatars,
0, true, avatar, SLOT(updateCollisionGroups()));
addCheckableActionToQMenuAndActionHash(collisionsMenu, MenuOption::CollideWithEnvironment,
0, false, avatar, SLOT(updateCollisionGroups()));
QMenu* viewMenu = addMenu("View");
#ifdef Q_OS_MAC

4
interface/src/Menu.h
View file

@ -127,10 +127,6 @@ namespace MenuOption {
const QString CascadedShadows = "Cascaded";
const QString CachesSize = "RAM Caches Size";
const QString Chat = "Chat...";
const QString ChatCircling = "Chat Circling";
const QString CollideAsRagdoll = "Collide With Self (Ragdoll)";
const QString CollideWithAvatars = "Collide With Other Avatars";
const QString CollideWithEnvironment = "Collide With World Boundaries";
const QString Collisions = "Collisions";
const QString Console = "Console...";
const QString ControlWithSpeech = "Control With Speech";

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

@ -74,7 +74,6 @@ Avatar::Avatar() :
_scale(1.0f),
_worldUpDirection(DEFAULT_UP_DIRECTION),
_moving(false),
_collisionGroups(0),
_initialized(false),
_shouldRenderBillboard(true),
_voiceSphereID(GeometryCache::UNKNOWN_ID)
@ -999,16 +998,6 @@ void Avatar::renderJointConnectingCone(glm::vec3 position1, glm::vec3 position2,
}
}
void Avatar::updateCollisionGroups() {
_collisionGroups = 0;
if (Menu::getInstance()->isOptionChecked(MenuOption::CollideWithEnvironment)) {
_collisionGroups |= COLLISION_GROUP_ENVIRONMENT;
}
if (Menu::getInstance()->isOptionChecked(MenuOption::CollideWithAvatars)) {
_collisionGroups |= COLLISION_GROUP_AVATARS;
}
}
void Avatar::setScale(float scale) {
_scale = scale;

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

@ -59,7 +59,6 @@ class Texture;
class Avatar : public AvatarData {
Q_OBJECT
Q_PROPERTY(quint32 collisionGroups READ getCollisionGroups WRITE setCollisionGroups)
Q_PROPERTY(glm::vec3 skeletonOffset READ getSkeletonOffset WRITE setSkeletonOffset)
public:
@ -137,9 +136,6 @@ public:
/// \return bounding radius of avatar
virtual float getBoundingRadius() const;
quint32 getCollisionGroups() const { return _collisionGroups; }
virtual void setCollisionGroups(quint32 collisionGroups) { _collisionGroups = (collisionGroups & VALID_COLLISION_GROUPS); }
Q_INVOKABLE void setSkeletonOffset(const glm::vec3& offset);
Q_INVOKABLE glm::vec3 getSkeletonOffset() { return _skeletonOffset; }
virtual glm::vec3 getSkeletonPosition() const;
@ -171,9 +167,6 @@ public:
// (otherwise floating point error will cause problems at large positions).
void applyPositionDelta(const glm::vec3& delta);
public slots:
void updateCollisionGroups();
signals:
void collisionWithAvatar(const QUuid& myUUID, const QUuid& theirUUID, const CollisionInfo& collision);
@ -203,8 +196,6 @@ protected:
float _stringLength;
bool _moving; ///< set when position is changing
quint32 _collisionGroups;
// protected methods...
glm::vec3 getBodyRightDirection() const { return getOrientation() * IDENTITY_RIGHT; }
glm::vec3 getBodyUpDirection() const { return getOrientation() * IDENTITY_UP; }

33
interface/src/avatar/MuscleConstraint.cpp
View file

@ -1,33 +0,0 @@
//
// MuscleConstraint.cpp
// interface/src/avatar
//
// Created by Andrew Meadows 2014.07.24
// 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
//
#include <SharedUtil.h>
#include <VerletPoint.h>
#include "MuscleConstraint.h"
const float DEFAULT_MUSCLE_STRENGTH = 0.5f * MAX_MUSCLE_STRENGTH;
MuscleConstraint::MuscleConstraint(VerletPoint* parent, VerletPoint* child) : _rootPoint(parent), _childPoint(child),
_parentIndex(-1), _childndex(-1), _strength(DEFAULT_MUSCLE_STRENGTH) {
_childOffset = child->_position - parent->_position;
}
float MuscleConstraint::enforce() {
_childPoint->_position += _strength * (_rootPoint->_position + _childOffset - _childPoint->_position);
return 0.0f;
}
void MuscleConstraint::setIndices(int parent, int child) {
_parentIndex = parent;
_childndex = child;
}

43
interface/src/avatar/MuscleConstraint.h
View file

@ -1,43 +0,0 @@
//
// MuscleConstraint.h
// interface/src/avatar
//
// Created by Andrew Meadows 2014.07.24
// 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_MuscleConstraint_h
#define hifi_MuscleConstraint_h
#include <Constraint.h>
// MuscleConstraint is a simple constraint that pushes the child toward an offset relative to the parent.
// It does NOT push the parent.
const float MAX_MUSCLE_STRENGTH = 0.75f;
class MuscleConstraint : public Constraint {
public:
MuscleConstraint(VerletPoint* parent, VerletPoint* child);
MuscleConstraint(const MuscleConstraint& other);
float enforce();
void setIndices(int parent, int child);
int getParentIndex() const { return _parentIndex; }
int getChildIndex() const { return _childndex; }
void setChildOffset(const glm::vec3& offset) { _childOffset = offset; }
void setStrength(float strength) { _strength = glm::clamp(strength, 0.0f, MAX_MUSCLE_STRENGTH); }
float getStrength() const { return _strength; }
private:
VerletPoint* _rootPoint;
VerletPoint* _childPoint;
int _parentIndex;
int _childndex;
glm::vec3 _childOffset;
float _strength; // a value in range [0,MAX_MUSCLE_STRENGTH]
};
#endif // hifi_MuscleConstraint_h

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

@ -72,7 +72,6 @@ MyAvatar::MyAvatar() :
Avatar(),
_turningKeyPressTime(0.0f),
_gravity(0.0f, 0.0f, 0.0f),
_distanceToNearestAvatar(std::numeric_limits<float>::max()),
_shouldJump(false),
_wasPushing(false),
_isPushing(false),
@ -88,7 +87,6 @@ MyAvatar::MyAvatar() :
_lookAtTargetAvatar(),
_shouldRender(true),
_billboardValid(false),
_physicsSimulation(),
_feetTouchFloor(true),
_isLookingAtLeftEye(true),
_realWorldFieldOfView("realWorldFieldOfView",
@ -98,18 +96,15 @@ MyAvatar::MyAvatar() :
for (int i = 0; i < MAX_DRIVE_KEYS; i++) {
_driveKeys[i] = 0.0f;
}
_physicsSimulation.setEntity(&_skeletonModel);
_skeletonModel.setEnableShapes(true);
_skeletonModel.buildRagdoll();
// connect to AddressManager signal for location jumps
connect(DependencyManager::get<AddressManager>().data(), &AddressManager::locationChangeRequired,
this, &MyAvatar::goToLocation);
}
MyAvatar::~MyAvatar() {
_physicsSimulation.clear();
_lookAtTargetAvatar.clear();
}
@ -176,7 +171,6 @@ void MyAvatar::simulate(float deltaTime) {
setScale(scale);
Application::getInstance()->getCamera()->setScale(scale);
}
_skeletonModel.setShowTrueJointTransforms(! Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll));
{
PerformanceTimer perfTimer("transform");
@ -213,16 +207,9 @@ void MyAvatar::simulate(float deltaTime) {
PerformanceTimer perfTimer("joints");
// copy out the skeleton joints from the model
_jointData.resize(_skeletonModel.getJointStateCount());
if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
for (int i = 0; i < _jointData.size(); i++) {
JointData& data = _jointData[i];
data.valid = _skeletonModel.getVisibleJointState(i, data.rotation);
}
} else {
for (int i = 0; i < _jointData.size(); i++) {
JointData& data = _jointData[i];
data.valid = _skeletonModel.getJointState(i, data.rotation);
}
for (int i = 0; i < _jointData.size(); i++) {
JointData& data = _jointData[i];
data.valid = _skeletonModel.getJointState(i, data.rotation);
}
}
@ -238,54 +225,6 @@ void MyAvatar::simulate(float deltaTime) {
head->simulate(deltaTime, true);
}
{
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)) {
// 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;
float length2 = glm::length2(ragdollDisplacement);
if (length2 > EPSILON && length2 < MAX_RAGDOLL_DISPLACEMENT_2) {
applyPositionDelta(ragdollDisplacement);
}
} else {
_skeletonModel.moveShapesTowardJoints(1.0f);
}
}
// now that we're done stepping the avatar forward in time, compute new collisions
if (_collisionGroups != 0) {
PerformanceTimer perfTimer("collisions");
Camera* myCamera = Application::getInstance()->getCamera();
float radius = getSkeletonHeight() * COLLISION_RADIUS_SCALE;
if (myCamera->getMode() == CAMERA_MODE_FIRST_PERSON && !OculusManager::isConnected()) {
radius = myCamera->getAspectRatio() * (myCamera->getNearClip() / cos(myCamera->getFieldOfView() / 2.0f));
radius *= COLLISION_RADIUS_SCALAR;
}
if (_collisionGroups & COLLISION_GROUP_ENVIRONMENT) {
PerformanceTimer perfTimer("environment");
updateCollisionWithEnvironment(deltaTime, radius);
}
if (_collisionGroups & COLLISION_GROUP_VOXELS) {
PerformanceTimer perfTimer("voxels");
updateCollisionWithVoxels(deltaTime, radius);
} else {
_trapDuration = 0.0f;
}
if (_collisionGroups & COLLISION_GROUP_AVATARS) {
PerformanceTimer perfTimer("avatars");
updateCollisionWithAvatars(deltaTime);
}
}
// Record avatars movements.
if (_recorder && _recorder->isRecording()) {
_recorder->record();
@ -1305,14 +1244,6 @@ void MyAvatar::updatePosition(float deltaTime) {
intersection._rayStart = glm::vec3(0.0f);
intersection._rayDirection = - _worldUpDirection;
intersection._rayLength = 4.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()
+ _skeletonModel.getBoundingShapeOffset().y;
if (heightAboveFloor < maxFloorDistance) {
hasFloor = true;
}
}
// velocity is initialized to the measured _velocity but will be modified by friction, external thrust, etc
glm::vec3 velocity = _velocity;
@ -1397,7 +1328,6 @@ void MyAvatar::updatePosition(float deltaTime) {
const float MOVING_SPEED_THRESHOLD = 0.01f;
_moving = speed > MOVING_SPEED_THRESHOLD;
updateChatCircle(deltaTime);
measureMotionDerivatives(deltaTime);
}
@ -1420,169 +1350,6 @@ void MyAvatar::updatePositionWithPhysics(float deltaTime) {
_velocity = rotation * newLocalVelocity;
}
void MyAvatar::updateCollisionWithEnvironment(float deltaTime, float radius) {
glm::vec3 up = getBodyUpDirection();
const float ENVIRONMENT_SURFACE_ELASTICITY = 0.0f;
const float ENVIRONMENT_SURFACE_DAMPING = 0.01f;
const float ENVIRONMENT_COLLISION_FREQUENCY = 0.05f;
glm::vec3 penetration;
float pelvisFloatingHeight = getPelvisFloatingHeight();
if (Application::getInstance()->getEnvironment()->findCapsulePenetration(
_position - up * (pelvisFloatingHeight - radius),
_position + up * (getSkeletonHeight() - pelvisFloatingHeight + radius), radius, penetration)) {
updateCollisionSound(penetration, deltaTime, ENVIRONMENT_COLLISION_FREQUENCY);
applyHardCollision(penetration, ENVIRONMENT_SURFACE_ELASTICITY, ENVIRONMENT_SURFACE_DAMPING);
}
}
static CollisionList myCollisions(64);
void MyAvatar::updateCollisionWithVoxels(float deltaTime, float radius) {
// 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) {
// don't even bother to try to collide against voxles when moving very fast
_trapDuration = 0.0f;
return;
}
bool isTrapped = false;
myCollisions.clear();
// 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;
const float capsuleRadius = boundingShape.getRadius();
const float capsuleHalfHeight = boundingShape.getHalfHeight();
const float MAX_STEP_HEIGHT = capsuleRadius + capsuleHalfHeight;
const float MIN_STEP_HEIGHT = 0.0f;
float highestStep = 0.0f;
float lowestStep = MAX_STEP_HEIGHT;
glm::vec3 floorPoint;
glm::vec3 stepPenetration(0.0f);
glm::vec3 totalPenetration(0.0f);
for (int i = 0; i < myCollisions.size(); ++i) {
CollisionInfo* collision = myCollisions[i];
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) {
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);
}
}
continue;
}
} else if (_trapDuration > MAX_TRAP_PERIOD) {
// 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) {
float stepHeight = - glm::dot(_worldUpDirection, collision->_penetration);
if (stepHeight > highestStep) {
highestStep = stepHeight;
stepPenetration = collision->_penetration;
}
if (stepHeight < lowestStep) {
lowestStep = stepHeight;
// remember that collision is in _physicsSimulation frame so we must add _position
floorPoint = _position + collision->_contactPoint - collision->_penetration;
}
}
}
float penetrationLength = glm::length(totalPenetration);
if (penetrationLength < EPSILON) {
_trapDuration = 0.0f;
return;
}
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 _keyboardMotorVelocity into world frame
glm::vec3 targetVelocity = _keyboardMotorVelocity;
glm::quat rotation = getHead()->getCameraOrientation();
targetVelocity = rotation * _keyboardMotorVelocity;
if (_wasPushing && glm::dot(targetVelocity, totalPenetration) > EPSILON) {
// we're puhing into the edge, so we want to lift
// remove unhelpful horizontal component of the step's penetration
totalPenetration -= stepPenetration - (glm::dot(stepPenetration, _worldUpDirection) * _worldUpDirection);
// further adjust penetration to help lift
float liftSpeed = glm::max(MAX_WALKING_SPEED, speed);
float thisStep = glm::min(liftSpeed * deltaTime, highestStep);
float extraStep = glm::dot(totalPenetration, _worldUpDirection) + thisStep;
if (extraStep > 0.0f) {
totalPenetration -= extraStep * _worldUpDirection;
}
_position -= totalPenetration;
} else {
// we're not pushing into the edge, so let the avatar fall
applyHardCollision(totalPenetration, VOXEL_ELASTICITY, VOXEL_DAMPING);
}
} else {
applyHardCollision(totalPenetration, VOXEL_ELASTICITY, VOXEL_DAMPING);
}
// Don't make a collision sound against voxlels by default -- too annoying when walking
//const float VOXEL_COLLISION_FREQUENCY = 0.5f;
//updateCollisionSound(myCollisions[0]->_penetration, deltaTime, VOXEL_COLLISION_FREQUENCY);
}
_trapDuration = isTrapped ? _trapDuration + deltaTime : 0.0f;
}
}
void MyAvatar::applyHardCollision(const glm::vec3& penetration, float elasticity, float damping) {
//
// Update the avatar in response to a hard collision. Position will be reset exactly
// to outside the colliding surface. Velocity will be modified according to elasticity.
//
// if elasticity = 0.0, collision is 100% inelastic.
// if elasticity = 1.0, collision is elastic.
//
_position -= penetration;
static float HALTING_VELOCITY = 0.2f;
// cancel out the velocity component in the direction of penetration
float penetrationLength = glm::length(penetration);
if (penetrationLength > EPSILON) {
glm::vec3 direction = penetration / penetrationLength;
_velocity -= glm::dot(_velocity, direction) * direction * (1.0f + elasticity);
_velocity *= glm::clamp(1.0f - damping, 0.0f, 1.0f);
if ((glm::length(_velocity) < HALTING_VELOCITY) && (glm::length(_thrust) == 0.0f)) {
// If moving really slowly after a collision, and not applying forces, stop altogether
_velocity *= 0.0f;
}
}
}
void MyAvatar::updateCollisionSound(const glm::vec3 &penetration, float deltaTime, float frequency) {
// COLLISION SOUND API in Audio has been removed
}
@ -1625,173 +1392,6 @@ bool findAvatarAvatarPenetration(const glm::vec3 positionA, float radiusA, float
return false;
}
void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
// Reset detector for nearest avatar
_distanceToNearestAvatar = std::numeric_limits<float>::max();
const AvatarHash& avatars = DependencyManager::get<AvatarManager>()->getAvatarHash();
if (avatars.size() <= 1) {
// no need to compute a bunch of stuff if we have one or fewer avatars
return;
}
float myBoundingRadius = getBoundingRadius();
// find nearest avatar
float nearestDistance2 = std::numeric_limits<float>::max();
Avatar* nearestAvatar = NULL;
foreach (const AvatarSharedPointer& avatarPointer, avatars) {
Avatar* avatar = static_cast<Avatar*>(avatarPointer.data());
if (static_cast<Avatar*>(this) == avatar) {
// don't collide with ourselves
continue;
}
float distance2 = glm::distance2(_position, avatar->getPosition());
if (nearestDistance2 > distance2) {
nearestDistance2 = distance2;
nearestAvatar = avatar;
}
}
_distanceToNearestAvatar = glm::sqrt(nearestDistance2);
if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
if (nearestAvatar != NULL) {
if (_distanceToNearestAvatar > myBoundingRadius + nearestAvatar->getBoundingRadius()) {
// they aren't close enough to put into the _physicsSimulation
// so we clear the pointer
nearestAvatar = NULL;
}
}
foreach (const AvatarSharedPointer& avatarPointer, avatars) {
Avatar* avatar = static_cast<Avatar*>(avatarPointer.data());
if (static_cast<Avatar*>(this) == avatar) {
// don't collide with ourselves
continue;
}
SkeletonModel* skeleton = &(avatar->getSkeletonModel());
PhysicsSimulation* simulation = skeleton->getSimulation();
if (avatar == nearestAvatar) {
if (simulation != &(_physicsSimulation)) {
skeleton->setEnableShapes(true);
_physicsSimulation.addEntity(skeleton);
_physicsSimulation.addRagdoll(skeleton->getRagdoll());
}
} else if (simulation == &(_physicsSimulation)) {
_physicsSimulation.removeRagdoll(skeleton->getRagdoll());
_physicsSimulation.removeEntity(skeleton);
skeleton->setEnableShapes(false);
}
}
}
}
class SortedAvatar {
public:
Avatar* avatar;
float distance;
glm::vec3 accumulatedCenter;
};
bool operator<(const SortedAvatar& s1, const SortedAvatar& s2) {
return s1.distance < s2.distance;
}
void MyAvatar::updateChatCircle(float deltaTime) {
if (!(_isChatCirclingEnabled = Menu::getInstance()->isOptionChecked(MenuOption::ChatCircling))) {
return;
}
// find all circle-enabled members and sort by distance
QVector<SortedAvatar> sortedAvatars;
foreach (const AvatarSharedPointer& avatarPointer, DependencyManager::get<AvatarManager>()->getAvatarHash()) {
Avatar* avatar = static_cast<Avatar*>(avatarPointer.data());
if ( ! avatar->isChatCirclingEnabled() ||
avatar == static_cast<Avatar*>(this)) {
continue;
}
SortedAvatar sortedAvatar;
sortedAvatar.avatar = avatar;
sortedAvatar.distance = glm::distance(_position, sortedAvatar.avatar->getPosition());
sortedAvatars.append(sortedAvatar);
}
qSort(sortedAvatars.begin(), sortedAvatars.end());
// compute the accumulated centers
glm::vec3 center = _position;
for (int i = 0; i < sortedAvatars.size(); i++) {
SortedAvatar& sortedAvatar = sortedAvatars[i];
sortedAvatar.accumulatedCenter = (center += sortedAvatar.avatar->getPosition()) / (i + 2.0f);
}
// remove members whose accumulated circles are too far away to influence us
const float CIRCUMFERENCE_PER_MEMBER = 0.5f;
const float CIRCLE_INFLUENCE_SCALE = 2.0f;
const float MIN_RADIUS = 0.3f;
for (int i = sortedAvatars.size() - 1; i >= 0; i--) {
float radius = qMax(MIN_RADIUS, (CIRCUMFERENCE_PER_MEMBER * (i + 2)) / TWO_PI);
if (glm::distance(_position, sortedAvatars[i].accumulatedCenter) > radius * CIRCLE_INFLUENCE_SCALE) {
sortedAvatars.remove(i);
} else {
break;
}
}
if (sortedAvatars.isEmpty()) {
return;
}
center = sortedAvatars.last().accumulatedCenter;
float radius = qMax(MIN_RADIUS, (CIRCUMFERENCE_PER_MEMBER * (sortedAvatars.size() + 1)) / TWO_PI);
// compute the average up vector
glm::vec3 up = getWorldAlignedOrientation() * IDENTITY_UP;
foreach (const SortedAvatar& sortedAvatar, sortedAvatars) {
up += sortedAvatar.avatar->getWorldAlignedOrientation() * IDENTITY_UP;
}
up = glm::normalize(up);
// find reasonable corresponding right/front vectors
glm::vec3 front = glm::cross(up, IDENTITY_RIGHT);
if (glm::length(front) < EPSILON) {
front = glm::cross(up, IDENTITY_FRONT);
}
front = glm::normalize(front);
glm::vec3 right = glm::cross(front, up);
// find our angle and the angular distances to our closest neighbors
glm::vec3 delta = _position - center;
glm::vec3 projected = glm::vec3(glm::dot(right, delta), glm::dot(front, delta), 0.0f);
float myAngle = glm::length(projected) > EPSILON ? atan2f(projected.y, projected.x) : 0.0f;
float leftDistance = TWO_PI;
float rightDistance = TWO_PI;
foreach (const SortedAvatar& sortedAvatar, sortedAvatars) {
delta = sortedAvatar.avatar->getPosition() - center;
projected = glm::vec3(glm::dot(right, delta), glm::dot(front, delta), 0.0f);
float angle = glm::length(projected) > EPSILON ? atan2f(projected.y, projected.x) : 0.0f;
if (angle < myAngle) {
leftDistance = min(myAngle - angle, leftDistance);
rightDistance = min(TWO_PI - (myAngle - angle), rightDistance);
} else {
leftDistance = min(TWO_PI - (angle - myAngle), leftDistance);
rightDistance = min(angle - myAngle, rightDistance);
}
}
// if we're on top of a neighbor, we need to randomize so that they don't both go in the same direction
if (rightDistance == 0.0f && randomBoolean()) {
swap(leftDistance, rightDistance);
}
// split the difference between our neighbors
float targetAngle = myAngle + (rightDistance - leftDistance) / 4.0f;
glm::vec3 targetPosition = center + (front * sinf(targetAngle) + right * cosf(targetAngle)) * radius;
// approach the target position
const float APPROACH_RATE = 0.05f;
_position = glm::mix(_position, targetPosition, APPROACH_RATE);
}
void MyAvatar::maybeUpdateBillboard() {
if (_billboardValid || !(_skeletonModel.isLoadedWithTextures() && getHead()->getFaceModel().isLoadedWithTextures())) {
return;
@ -1894,17 +1494,6 @@ void MyAvatar::updateMotionBehavior() {
_feetTouchFloor = menu->isOptionChecked(MenuOption::ShiftHipsForIdleAnimations);
}
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, RenderArgs* args) {
if (Application::getInstance()->getCamera()->getMode() != CAMERA_MODE_FIRST_PERSON || renderMode == MIRROR_RENDER_MODE) {
Avatar::renderAttachments(renderMode, args);
@ -1922,38 +1511,6 @@ void MyAvatar::renderAttachments(RenderMode renderMode, RenderArgs* args) {
}
}
void MyAvatar::setCollisionGroups(quint32 collisionGroups) {
Avatar::setCollisionGroups(collisionGroups & VALID_COLLISION_GROUPS);
Menu* menu = Menu::getInstance();
menu->setIsOptionChecked(MenuOption::CollideWithEnvironment, (bool)(_collisionGroups & COLLISION_GROUP_ENVIRONMENT));
menu->setIsOptionChecked(MenuOption::CollideWithAvatars, (bool)(_collisionGroups & COLLISION_GROUP_AVATARS));
// TODO: what to do about this now that voxels are gone
if (! (_collisionGroups & COLLISION_GROUP_VOXELS)) {
// no collision with voxels --> disable standing on floors
_motionBehaviors &= ~AVATAR_MOTION_STAND_ON_NEARBY_FLOORS;
menu->setIsOptionChecked(MenuOption::StandOnNearbyFloors, false);
}
}
void MyAvatar::applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) {
glm::vec3 leverAxis = contactPoint - getPosition();
float leverLength = glm::length(leverAxis);
if (leverLength > EPSILON) {
// compute lean perturbation angles
glm::quat bodyRotation = getOrientation();
glm::vec3 xAxis = bodyRotation * glm::vec3(1.0f, 0.0f, 0.0f);
glm::vec3 zAxis = bodyRotation * glm::vec3(0.0f, 0.0f, 1.0f);
leverAxis = leverAxis / leverLength;
glm::vec3 effectivePenetration = penetration - glm::dot(penetration, leverAxis) * leverAxis;
// use the small-angle approximation for sine
float sideways = - glm::dot(effectivePenetration, xAxis) / leverLength;
float forward = glm::dot(effectivePenetration, zAxis) / leverLength;
getHead()->addLeanDeltas(sideways, forward);
}
}
//Renders sixense laser pointers for UI selection with controllers
void MyAvatar::renderLaserPointers() {
const float PALM_TIP_ROD_RADIUS = 0.002f;

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

@ -12,7 +12,6 @@
#ifndef hifi_MyAvatar_h
#define hifi_MyAvatar_h
#include <PhysicsSimulation.h>
#include <SettingHandle.h>
#include "Avatar.h"
@ -139,10 +138,6 @@ public:
const glm::vec3& translation = glm::vec3(), const glm::quat& rotation = glm::quat(), float scale = 1.0f,
bool allowDuplicates = false, bool useSaved = true);
virtual void setCollisionGroups(quint32 collisionGroups);
void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration);
/// Renders a laser pointer for UI picking
void renderLaserPointers();
glm::vec3 getLaserPointerTipPosition(const PalmData* palm);
@ -165,7 +160,6 @@ public slots:
void setThrust(glm::vec3 newThrust) { _thrust = newThrust; }
void updateMotionBehavior();
void onToggleRagdoll();
glm::vec3 getLeftPalmPosition();
glm::vec3 getRightPalmPosition();
@ -190,7 +184,6 @@ protected:
private:
float _turningKeyPressTime;
glm::vec3 _gravity;
float _distanceToNearestAvatar; // How close is the nearest avatar?
bool _shouldJump;
float _driveKeys[MAX_DRIVE_KEYS];
@ -215,7 +208,6 @@ private:
float _oculusYawOffset;
QList<AnimationHandlePointer> _animationHandles;
PhysicsSimulation _physicsSimulation;
bool _feetTouchFloor;
bool _isLookingAtLeftEye;
@ -232,12 +224,7 @@ private:
glm::vec3 applyScriptedMotor(float deltaTime, const glm::vec3& velocity);
void updatePosition(float deltaTime);
void updatePositionWithPhysics(float deltaTime);
void updateCollisionWithAvatars(float deltaTime);
void updateCollisionWithEnvironment(float deltaTime, float radius);
void updateCollisionWithVoxels(float deltaTime, float radius);
void applyHardCollision(const glm::vec3& penetration, float elasticity, float damping);
void updateCollisionSound(const glm::vec3& penetration, float deltaTime, float frequency);
void updateChatCircle(float deltaTime);
void maybeUpdateBillboard();
void setGravity(const glm::vec3& gravity);
};

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

@ -12,15 +12,14 @@
#include <glm/gtx/transform.hpp>
#include <QMultiMap>
#include <VerletCapsuleShape.h>
#include <VerletSphereShape.h>
#include <CapsuleShape.h>
#include <SphereShape.h>
#include "Application.h"
#include "Avatar.h"
#include "Hand.h"
#include "Menu.h"
#include "SkeletonModel.h"
#include "SkeletonRagdoll.h"
#include "Util.h"
enum StandingFootState {
@ -35,7 +34,6 @@ SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent) :
_owningAvatar(owningAvatar),
_boundingShape(),
_boundingShapeLocalOffset(0.0f),
_ragdoll(NULL),
_defaultEyeModelPosition(glm::vec3(0.0f, 0.0f, 0.0f)),
_standingFoot(NO_FOOT),
_standingOffset(0.0f),
@ -44,8 +42,6 @@ SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent) :
}
SkeletonModel::~SkeletonModel() {
delete _ragdoll;
_ragdoll = NULL;
}
void SkeletonModel::setJointStates(QVector<JointState> states) {
@ -175,12 +171,6 @@ void SkeletonModel::getHandShapes(int jointIndex, QVector<const Shape*>& shapes)
}
}
void SkeletonModel::getBodyShapes(QVector<const Shape*>& shapes) const {
// for now we push a single bounding shape,
// but later we could push a subset of joint shapes
shapes.push_back(&_boundingShape);
}
void SkeletonModel::renderIKConstraints() {
renderJointConstraints(getRightHandJointIndex());
renderJointConstraints(getLeftHandJointIndex());
@ -496,10 +486,6 @@ bool SkeletonModel::getHeadPosition(glm::vec3& headPosition) const {
}
bool SkeletonModel::getNeckPosition(glm::vec3& neckPosition) const {
if (_owningAvatar->isMyAvatar() &&
Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
return isActive() && getVisibleJointPositionInWorldFrame(_geometry->getFBXGeometry().neckJointIndex, neckPosition);
}
return isActive() && getJointPositionInWorldFrame(_geometry->getFBXGeometry().neckJointIndex, neckPosition);
}
@ -513,12 +499,7 @@ bool SkeletonModel::getNeckParentRotationFromDefaultOrientation(glm::quat& neckP
}
int parentIndex = geometry.joints.at(geometry.neckJointIndex).parentIndex;
glm::quat worldFrameRotation;
bool success = false;
if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
success = getVisibleJointRotationInWorldFrame(parentIndex, worldFrameRotation);
} else {
success = getJointRotationInWorldFrame(parentIndex, worldFrameRotation);
}
bool success = getJointRotationInWorldFrame(parentIndex, worldFrameRotation);
if (success) {
neckParentRotation = worldFrameRotation * _jointStates[parentIndex].getFBXJoint().inverseDefaultRotation;
}
@ -565,83 +546,6 @@ glm::vec3 SkeletonModel::getDefaultEyeModelPosition() const {
return _owningAvatar->getScale() * _defaultEyeModelPosition;
}
void SkeletonModel::renderRagdoll() {
if (!_ragdoll) {
return;
}
const QVector<VerletPoint>& points = _ragdoll->getPoints();
const int BALL_SUBDIVISIONS = 6;
glDisable(GL_DEPTH_TEST);
glDisable(GL_LIGHTING);
glPushMatrix();
Application::getInstance()->loadTranslatedViewMatrix(_translation);
int numPoints = points.size();
float alpha = 0.3f;
float radius1 = 0.008f;
float radius2 = 0.01f;
glm::vec3 simulationTranslation = _ragdoll->getTranslationInSimulationFrame();
auto geometryCache = DependencyManager::get<GeometryCache>();
for (int i = 0; i < numPoints; ++i) {
glPushMatrix();
// NOTE: ragdollPoints are in simulation-frame but we want them to be model-relative
glm::vec3 position = points[i]._position - simulationTranslation;
glTranslatef(position.x, position.y, position.z);
// draw each point as a yellow hexagon with black border
geometryCache->renderSphere(radius2, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS, glm::vec4(0.0f, 0.0f, 0.0f, alpha));
geometryCache->renderSphere(radius1, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS, glm::vec4(1.0f, 1.0f, 0.0f, alpha));
glPopMatrix();
}
glPopMatrix();
glEnable(GL_DEPTH_TEST);
glEnable(GL_LIGHTING);
}
void SkeletonModel::updateVisibleJointStates() {
if (_showTrueJointTransforms || !_ragdoll) {
// no need to update visible transforms
return;
}
const QVector<VerletPoint>& ragdollPoints = _ragdoll->getPoints();
QVector<glm::vec3> points;
points.reserve(_jointStates.size());
glm::quat invRotation = glm::inverse(_rotation);
for (int i = 0; i < _jointStates.size(); i++) {
JointState& state = _jointStates[i];
points.push_back(ragdollPoints[i]._position);
// get the parent state (this is the state that we want to rotate)
int parentIndex = state.getParentIndex();
if (parentIndex == -1) {
_jointStates[i].slaveVisibleTransform();
continue;
}
JointState& parentState = _jointStates[parentIndex];
// check the grand-parent index (for now we don't want to rotate any root states)
int grandParentIndex = parentState.getParentIndex();
if (grandParentIndex == -1) {
continue;
}
// make sure state's visibleTransform is up to date
const glm::mat4& parentTransform = parentState.getVisibleTransform();
state.computeVisibleTransform(parentTransform);
// we're looking for the rotation that moves visible bone parallel to ragdoll bone
// rotationBetween(jointTip - jointPivot, shapeTip - shapePivot)
// NOTE: points are in simulation-frame so rotate line segment into model-frame
glm::quat delta = rotationBetween(state.getVisiblePosition() - extractTranslation(parentTransform),
invRotation * (points[i] - points[parentIndex]));
// apply
parentState.mixVisibleRotationDelta(delta, 0.01f);
// update transforms
parentState.computeVisibleTransform(_jointStates[grandParentIndex].getVisibleTransform());
state.computeVisibleTransform(parentState.getVisibleTransform());
}
}
/// \return offset of hips after foot animation
void SkeletonModel::updateStandingFoot() {
if (_geometry == NULL) {
@ -701,17 +605,6 @@ void SkeletonModel::updateStandingFoot() {
_standingOffset = offset;
}
SkeletonRagdoll* SkeletonModel::buildRagdoll() {
if (!_ragdoll) {
_ragdoll = new SkeletonRagdoll(this);
if (_enableShapes) {
clearShapes();
buildShapes();
}
}
return _ragdoll;
}
float DENSITY_OF_WATER = 1000.0f; // kg/m^3
float MIN_JOINT_MASS = 1.0f;
float VERY_BIG_MASS = 1.0e6f;
@ -728,18 +621,8 @@ void SkeletonModel::buildShapes() {
return;
}
if (!_ragdoll) {
_ragdoll = new SkeletonRagdoll(this);
}
_ragdoll->setRootIndex(geometry.rootJointIndex);
_ragdoll->initPoints();
QVector<VerletPoint>& points = _ragdoll->getPoints();
float massScale = _ragdoll->getMassScale();
float uniformScale = extractUniformScale(_scale);
const int numStates = _jointStates.size();
float totalMass = 0.0f;
for (int i = 0; i < numStates; i++) {
JointState& state = _jointStates[i];
const FBXJoint& joint = state.getFBXJoint();
@ -755,18 +638,12 @@ void SkeletonModel::buildShapes() {
}
Shape* shape = NULL;
if (type == SPHERE_SHAPE) {
shape = new VerletSphereShape(radius, &(points[i]));
shape = new SphereShape(radius);
shape->setEntity(this);
float mass = massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume());
points[i].setMass(mass);
totalMass += mass;
} else if (type == CAPSULE_SHAPE) {
assert(parentIndex != -1);
shape = new VerletCapsuleShape(radius, &(points[parentIndex]), &(points[i]));
shape = new CapsuleShape(radius, halfHeight);
shape->setEntity(this);
float mass = massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume());
points[i].setMass(mass);
totalMass += mass;
}
if (shape && parentIndex != -1) {
// always disable collisions between joint and its parent
@ -775,47 +652,8 @@ void SkeletonModel::buildShapes() {
_shapes.push_back(shape);
}
// set the mass of the root
if (numStates > 0) {
points[_ragdoll->getRootIndex()].setMass(totalMass);
}
// This method moves the shapes to their default positions in Model frame.
computeBoundingShape(geometry);
// While the shapes are in their default position we disable collisions between
// joints that are currently colliding.
disableCurrentSelfCollisions();
_ragdoll->buildConstraints();
// ... then move shapes back to current joint positions
_ragdoll->slamPointPositions();
_ragdoll->enforceConstraints();
}
void SkeletonModel::moveShapesTowardJoints(float deltaTime) {
// KEEP: although we don't currently use this method we may eventually need it to help
// unravel a skelton that has become tangled in its constraints. So let's keep this
// around for a while just in case.
const int numStates = _jointStates.size();
QVector<VerletPoint>& ragdollPoints = _ragdoll->getPoints();
assert(_jointStates.size() == ragdollPoints.size());
if (ragdollPoints.size() != numStates) {
return;
}
// fraction = 0 means keep old position, = 1 means slave 100% to target position
const float RAGDOLL_FOLLOWS_JOINTS_TIMESCALE = 0.05f;
float fraction = glm::clamp(deltaTime / RAGDOLL_FOLLOWS_JOINTS_TIMESCALE, 0.0f, 1.0f);
float oneMinusFraction = 1.0f - fraction;
glm::vec3 simulationTranslation = _ragdoll->getTranslationInSimulationFrame();
for (int i = 0; i < numStates; ++i) {
// ragdollPoints are in simulation-frame but jointStates are in model-frame
ragdollPoints[i].initPosition(oneMinusFraction * ragdollPoints[i]._position +
fraction * (simulationTranslation + _rotation * (_jointStates.at(i).getPosition())));
}
}
void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
@ -824,25 +662,21 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
QVector<glm::mat4> transforms;
transforms.fill(glm::mat4(), numStates);
QVector<VerletPoint>& ragdollPoints = _ragdoll->getPoints();
// compute the default transforms and slam the ragdoll positions accordingly
// (which puts the shapes where we want them)
// compute the default transforms
for (int i = 0; i < numStates; i++) {
JointState& state = _jointStates[i];
const FBXJoint& joint = state.getFBXJoint();
int parentIndex = joint.parentIndex;
if (parentIndex == -1) {
transforms[i] = _jointStates[i].getTransform();
ragdollPoints[i].initPosition(extractTranslation(transforms[i]));
continue;
}
glm::quat modifiedRotation = joint.preRotation * joint.rotation * joint.postRotation;
transforms[i] = transforms[parentIndex] * glm::translate(joint.translation)
* joint.preTransform * glm::mat4_cast(modifiedRotation) * joint.postTransform;
// setting the ragdollPoints here slams the VerletShapes into their default positions
ragdollPoints[i].initPosition(extractTranslation(transforms[i]));
// TODO: Andrew to harvest transforms here to move shapes to correct positions so that
// bounding capsule calculations below are correct.
}
// compute bounding box that encloses all shapes
@ -959,56 +793,6 @@ void SkeletonModel::renderBoundingCollisionShapes(float alpha) {
const int BALL_SUBDIVISIONS = 10;
// virtual
void SkeletonModel::renderJointCollisionShapes(float alpha) {
if (!_ragdoll) {
return;
}
glPushMatrix();
Application::getInstance()->loadTranslatedViewMatrix(_translation);
glm::vec3 simulationTranslation = _ragdoll->getTranslationInSimulationFrame();
for (int i = 0; i < _shapes.size(); i++) {
Shape* shape = _shapes[i];
if (!shape) {
continue;
}
auto geometryCache = DependencyManager::get<GeometryCache>();
glPushMatrix();
// shapes are stored in simulation-frame but we want position to be model-relative
if (shape->getType() == SPHERE_SHAPE) {
glm::vec3 position = shape->getTranslation() - simulationTranslation;
glTranslatef(position.x, position.y, position.z);
// draw a grey sphere at shape position
geometryCache->renderSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS, glm::vec4(0.75f, 0.75f, 0.75f, alpha));
} else if (shape->getType() == CAPSULE_SHAPE) {
CapsuleShape* capsule = static_cast<CapsuleShape*>(shape);
// draw a blue sphere at the capsule endpoint
glm::vec3 endPoint;
capsule->getEndPoint(endPoint);
endPoint = endPoint - simulationTranslation;
glTranslatef(endPoint.x, endPoint.y, endPoint.z);
geometryCache->renderSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS, glm::vec4(0.6f, 0.6f, 0.8f, alpha));
// draw a yellow sphere at the capsule startpoint
glm::vec3 startPoint;
capsule->getStartPoint(startPoint);
startPoint = startPoint - simulationTranslation;
glm::vec3 axis = endPoint - startPoint;
glTranslatef(-axis.x, -axis.y, -axis.z);
geometryCache->renderSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS, glm::vec4(0.8f, 0.8f, 0.6f, alpha));
// draw a green cylinder between the two points
glm::vec3 origin(0.0f);
Avatar::renderJointConnectingCone( origin, axis, capsule->getRadius(), capsule->getRadius(), glm::vec4(0.6f, 0.8f, 0.6f, alpha));
}
glPopMatrix();
}
glPopMatrix();
}
bool SkeletonModel::hasSkeleton() {
return isActive() ? _geometry->getFBXGeometry().rootJointIndex != -1 : false;
}

17
interface/src/avatar/SkeletonModel.h
View file

@ -16,11 +16,8 @@
#include <CapsuleShape.h>
#include <Model.h>
#include "SkeletonRagdoll.h"
class Avatar;
class MuscleConstraint;
class SkeletonRagdoll;
/// A skeleton loaded from a model.
class SkeletonModel : public Model {
@ -39,9 +36,6 @@ public:
/// \param shapes[out] list in which is stored pointers to hand shapes
void getHandShapes(int jointIndex, QVector<const Shape*>& shapes) const;
/// \param shapes[out] list of shapes for body collisions
void getBodyShapes(QVector<const Shape*>& shapes) const;
void renderIKConstraints();
/// Returns the index of the left hand joint, or -1 if not found.
@ -106,24 +100,14 @@ public:
void updateStandingFoot();
const glm::vec3& getStandingOffset() const { return _standingOffset; }
virtual void updateVisibleJointStates();
SkeletonRagdoll* buildRagdoll();
SkeletonRagdoll* getRagdoll() { return _ragdoll; }
void moveShapesTowardJoints(float fraction);
void computeBoundingShape(const FBXGeometry& geometry);
void renderBoundingCollisionShapes(float alpha);
void renderJointCollisionShapes(float alpha);
float getBoundingShapeRadius() const { return _boundingShape.getRadius(); }
const CapsuleShape& getBoundingShape() const { return _boundingShape; }
const glm::vec3 getBoundingShapeOffset() const { return _boundingShapeLocalOffset; }
void resetShapePositionsToDefaultPose(); // DEBUG method
void renderRagdoll();
bool hasSkeleton();
signals:
@ -171,7 +155,6 @@ private:
CapsuleShape _boundingShape;
glm::vec3 _boundingShapeLocalOffset;
SkeletonRagdoll* _ragdoll;
glm::vec3 _defaultEyeModelPosition;
int _standingFoot;

145
interface/src/avatar/SkeletonRagdoll.cpp
View file

@ -1,145 +0,0 @@
//
// SkeletonRagdoll.cpp
// interface/src/avatar
//
// Created by Andrew Meadows 2014.08.14
// 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
//
#include <DistanceConstraint.h>
#include <FixedConstraint.h>
#include <Model.h>
#include "SkeletonRagdoll.h"
#include "MuscleConstraint.h"
SkeletonRagdoll::SkeletonRagdoll(Model* model) : Ragdoll(), _model(model) {
assert(_model);
}
SkeletonRagdoll::~SkeletonRagdoll() {
}
// virtual
void SkeletonRagdoll::stepForward(float deltaTime) {
setTransform(_model->getTranslation(), _model->getRotation());
Ragdoll::stepForward(deltaTime);
updateMuscles();
int numConstraints = _muscleConstraints.size();
for (int i = 0; i < numConstraints; ++i) {
_muscleConstraints[i]->enforce();
}
}
void SkeletonRagdoll::slamPointPositions() {
QVector<JointState>& jointStates = _model->getJointStates();
const int numPoints = _points.size();
assert(numPoints == jointStates.size());
for (int i = _rootIndex; i < numPoints; ++i) {
_points[i].initPosition(jointStates.at(i).getPosition());
}
}
// virtual
void SkeletonRagdoll::initPoints() {
clearConstraintsAndPoints();
_muscleConstraints.clear();
initTransform();
// one point for each joint
int numStates = _model->getJointStates().size();
_points.fill(VerletPoint(), numStates);
slamPointPositions();
}
// virtual
void SkeletonRagdoll::buildConstraints() {
QVector<JointState>& jointStates = _model->getJointStates();
// NOTE: the length of DistanceConstraints is computed and locked in at this time
// so make sure the ragdoll positions are in a normal configuration before here.
const int numPoints = _points.size();
assert(numPoints == jointStates.size());
float minBone = FLT_MAX;
float maxBone = -FLT_MAX;
QMultiMap<int, int> families;
for (int i = _rootIndex; i < numPoints; ++i) {
const JointState& state = jointStates.at(i);
int parentIndex = state.getParentIndex();
if (parentIndex != -1) {
DistanceConstraint* bone = new DistanceConstraint(&(_points[i]), &(_points[parentIndex]));
bone->setDistance(state.getDistanceToParent());
_boneConstraints.push_back(bone);
families.insert(parentIndex, i);
}
float boneLength = glm::length(state.getPositionInParentFrame());
if (boneLength > maxBone) {
maxBone = boneLength;
} else if (boneLength < minBone) {
minBone = boneLength;
}
}
// Joints that have multiple children effectively have rigid constraints between the children
// in the parent frame, so we add DistanceConstraints between children in the same family.
QMultiMap<int, int>::iterator itr = families.begin();
while (itr != families.end()) {
QList<int> children = families.values(itr.key());
int numChildren = children.size();
if (numChildren > 1) {
for (int i = 1; i < numChildren; ++i) {
DistanceConstraint* bone = new DistanceConstraint(&(_points[children[i-1]]), &(_points[children[i]]));
_boneConstraints.push_back(bone);
}
if (numChildren > 2) {
DistanceConstraint* bone = new DistanceConstraint(&(_points[children[numChildren-1]]), &(_points[children[0]]));
_boneConstraints.push_back(bone);
}
}
++itr;
}
float MAX_STRENGTH = 0.6f;
float MIN_STRENGTH = 0.05f;
// each joint gets a MuscleConstraint to its parent
for (int i = _rootIndex + 1; i < numPoints; ++i) {
const JointState& state = jointStates.at(i);
int p = state.getParentIndex();
if (p == -1) {
continue;
}
MuscleConstraint* constraint = new MuscleConstraint(&(_points[p]), &(_points[i]));
_muscleConstraints.push_back(constraint);
// Short joints are more susceptible to wiggle so we modulate the strength based on the joint's length:
// long = weak and short = strong.
constraint->setIndices(p, i);
float boneLength = glm::length(state.getPositionInParentFrame());
float strength = MIN_STRENGTH + (MAX_STRENGTH - MIN_STRENGTH) * (maxBone - boneLength) / (maxBone - minBone);
if (!families.contains(i)) {
// Although muscles only pull on the children not parents, nevertheless those joints that have
// parents AND children are more stable than joints at the end such as fingers. For such joints we
// bestow maximum strength which helps reduce wiggle.
strength = MAX_MUSCLE_STRENGTH;
}
constraint->setStrength(strength);
}
}
void SkeletonRagdoll::updateMuscles() {
QVector<JointState>& jointStates = _model->getJointStates();
int numConstraints = _muscleConstraints.size();
glm::quat rotation = _model->getRotation();
for (int i = 0; i < numConstraints; ++i) {
MuscleConstraint* constraint = _muscleConstraints[i];
int j = constraint->getParentIndex();
int k = constraint->getChildIndex();
assert(j != -1 && k != -1);
// ragdollPoints are in simulation-frame but jointStates are in model-frame
constraint->setChildOffset(rotation * (jointStates.at(k).getPosition() - jointStates.at(j).getPosition()));
}
}

43
interface/src/avatar/SkeletonRagdoll.h
View file

@ -1,43 +0,0 @@
//
// SkeletonkRagdoll.h
// interface/src/avatar
//
// Created by Andrew Meadows 2014.08.14
// 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_SkeletonRagdoll_h
#define hifi_SkeletonRagdoll_h
#include <QVector>
#include <JointState.h>
#include <Ragdoll.h>
class MuscleConstraint;
class Model;
class SkeletonRagdoll : public Ragdoll {
public:
SkeletonRagdoll(Model* model);
virtual ~SkeletonRagdoll();
void slamPointPositions();
virtual void stepForward(float deltaTime);
virtual void initPoints();
virtual void buildConstraints();
protected:
void updateMuscles();
private:
Model* _model;
QVector<MuscleConstraint*> _muscleConstraints;
};
#endif // hifi_SkeletonRagdoll_h

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

@ -43,7 +43,6 @@ AvatarData::AvatarData() :
_targetScale(1.0f),
_handState(0),
_keyState(NO_KEY_DOWN),
_isChatCirclingEnabled(false),
_forceFaceTrackerConnected(false),
_hasNewJointRotations(true),
_headData(NULL),
@ -195,9 +194,6 @@ QByteArray AvatarData::toByteArray() {
if (_headData->_isFaceTrackerConnected) {
setAtBit(bitItems, IS_FACESHIFT_CONNECTED);
}
if (_isChatCirclingEnabled) {
setAtBit(bitItems, IS_CHAT_CIRCLING_ENABLED);
}
if (_referential != NULL && _referential->isValid()) {
setAtBit(bitItems, HAS_REFERENTIAL);
}
@ -419,7 +415,6 @@ int AvatarData::parseDataAtOffset(const QByteArray& packet, int offset) {
+ (oneAtBit(bitItems, HAND_STATE_FINGER_POINTING_BIT) ? IS_FINGER_POINTING_FLAG : 0);
_headData->_isFaceTrackerConnected = oneAtBit(bitItems, IS_FACESHIFT_CONNECTED);
_isChatCirclingEnabled = oneAtBit(bitItems, IS_CHAT_CIRCLING_ENABLED);
bool hasReferential = oneAtBit(bitItems, HAS_REFERENTIAL);
// Referential

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

@ -91,7 +91,7 @@ const quint32 AVATAR_MOTION_SCRIPTABLE_BITS =
const int KEY_STATE_START_BIT = 0; // 1st and 2nd bits
const int HAND_STATE_START_BIT = 2; // 3rd and 4th bits
const int IS_FACESHIFT_CONNECTED = 4; // 5th bit
const int IS_CHAT_CIRCLING_ENABLED = 5; // 6th bit
const int UNUSED_AVATAR_STATE_BIT_5 = 5; // 6th bit (was CHAT_CIRCLING)
const int HAS_REFERENTIAL = 6; // 7th bit
const int HAND_STATE_FINGER_POINTING_BIT = 7; // 8th bit
@ -248,7 +248,6 @@ public:
void setKeyState(KeyState s) { _keyState = s; }
KeyState keyState() const { return _keyState; }
bool isChatCirclingEnabled() const { return _isChatCirclingEnabled; }
const HeadData* getHeadData() const { return _headData; }
const HandData* getHandData() const { return _handData; }
@ -370,7 +369,6 @@ protected:
// key state
KeyState _keyState;
bool _isChatCirclingEnabled;
bool _forceFaceTrackerConnected;
bool _hasNewJointRotations; // set in AvatarData, cleared in Avatar

53
libraries/physics/src/ContactConstraint.cpp
View file

@ -1,53 +0,0 @@
//
// ContactConstraint.cpp
// libraries/physcis/src
//
// Created by Andrew Meadows 2014.07.24
// 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
//
#include <SharedUtil.h>
#include "ContactConstraint.h"
#include "VerletPoint.h"
ContactConstraint::ContactConstraint(VerletPoint* pointA, VerletPoint* pointB)
: _pointA(pointA), _pointB(pointB), _strength(1.0f) {
assert(_pointA != NULL && _pointB != NULL);
_offset = _pointB->_position - _pointA->_position;
}
float ContactConstraint::enforce() {
_pointB->_position += _strength * (_pointA->_position + _offset - _pointB->_position);
return 0.0f;
}
float ContactConstraint::enforceWithNormal(const glm::vec3& normal) {
glm::vec3 delta = _pointA->_position + _offset - _pointB->_position;
// split delta into parallel (pDelta) and perpendicular (qDelta) components
glm::vec3 pDelta = glm::dot(delta, normal) * normal;
glm::vec3 qDelta = delta - pDelta;
// use the relative sizes of the components to decide how much perpenducular delta to use
// (i.e. dynamic friction)
float lpDelta = glm::length(pDelta);
float lqDelta = glm::length(qDelta);
float qFactor = lqDelta > lpDelta ? (lpDelta / lqDelta - 1.0f) : 0.0f;
// recombine the two components to get the final delta
delta = pDelta + qFactor * qDelta;
// attenuate strength by how much _offset is perpendicular to normal
float distance = glm::length(_offset);
float strength = _strength * ((distance > EPSILON) ? glm::abs(glm::dot(_offset, normal)) / distance : 1.0f);
// move _pointB
_pointB->_position += strength * delta;
return strength * glm::length(delta);
}

39
libraries/physics/src/ContactConstraint.h
View file

@ -1,39 +0,0 @@
//
// ContactConstraint.h
// libraries/physcis/src
//
// Created by Andrew Meadows 2014.07.24
// 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_ContactConstraint_h
#define hifi_ContactConstraint_h
#include <glm/glm.hpp>
#include "Constraint.h"
#include "VerletPoint.h"
class ContactConstraint : public Constraint {
public:
ContactConstraint(VerletPoint* pointA, VerletPoint* pointB);
float enforce();
float enforceWithNormal(const glm::vec3& normal);
glm::vec3 getTargetPointA() const { return _pointB->_position - _offset; }
void setOffset(const glm::vec3& offset) { _offset = offset; }
void setStrength(float strength) { _strength = glm::clamp(strength, 0.0f, 1.0f); }
float getStrength() const { return _strength; }
private:
VerletPoint* _pointA;
VerletPoint* _pointB;
glm::vec3 _offset; // from pointA toward pointB
float _strength; // a value in range [0,1]
};
#endif // hifi_ContactConstraint_h

216
libraries/physics/src/ContactPoint.cpp
View file

@ -1,216 +0,0 @@
//
// ContactPoint.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2014.07.30
// 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
//
#include <SharedUtil.h>
#include "ContactPoint.h"
#include "Shape.h"
// This parameter helps keep the actual point of contact slightly inside each shape
// which allows the collisions to happen almost every frame for more frequent updates.
const float CONTACT_PENETRATION_ALLOWANCE = 0.005f;
ContactPoint::ContactPoint() :
_lastFrame(0), _shapeA(NULL), _shapeB(NULL),
_offsetA(0.0f), _offsetB(0.0f),
_relativeMassA(0.5f), _relativeMassB(0.5f),
_numPointsA(0), _numPoints(0), _normal(0.0f) {
}
ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) :
_lastFrame(frame), _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()),
_offsetA(0.0f), _offsetB(0.0f),
_relativeMassA(0.5f), _relativeMassB(0.5f),
_numPointsA(0), _numPoints(0), _normal(0.0f) {
glm::vec3 pointA = collision._contactPoint;
glm::vec3 pointB = collision._contactPoint - collision._penetration;
float pLength = glm::length(collision._penetration);
if (pLength > EPSILON) {
_normal = collision._penetration / pLength;
}
if (_shapeA->getID() > _shapeB->getID()) {
// swap so that _shapeA always has lower ID
_shapeA = collision.getShapeB();
_shapeB = collision.getShapeA();
_normal = - _normal;
pointA = pointB;
pointB = collision._contactPoint;
}
// bring the contact points inside the shapes to help maintain collision updates
pointA -= CONTACT_PENETRATION_ALLOWANCE * _normal;
pointB += CONTACT_PENETRATION_ALLOWANCE * _normal;
_offsetA = pointA - _shapeA->getTranslation();
_offsetB = pointB - _shapeB->getTranslation();
_shapeA->getVerletPoints(_points);
_numPointsA = _points.size();
_shapeB->getVerletPoints(_points);
_numPoints = _points.size();
// compute and cache relative masses
float massA = EPSILON;
for (int i = 0; i < _numPointsA; ++i) {
massA += _points[i]->getMass();
}
float massB = EPSILON;
for (int i = _numPointsA; i < _numPoints; ++i) {
massB += _points[i]->getMass();
}
float invTotalMass = 1.0f / (massA + massB);
_relativeMassA = massA * invTotalMass;
_relativeMassB = massB * invTotalMass;
// _contactPoint will be the weighted average of the two
_contactPoint = _relativeMassA * pointA + _relativeMassB * pointB;
// compute offsets for shapeA
for (int i = 0; i < _numPointsA; ++i) {
glm::vec3 offset = _points[i]->_position - pointA;
_offsets.push_back(offset);
_distances.push_back(glm::length(offset));
}
// compute offsets for shapeB
for (int i = _numPointsA; i < _numPoints; ++i) {
glm::vec3 offset = _points[i]->_position - pointB;
_offsets.push_back(offset);
_distances.push_back(glm::length(offset));
}
}
float ContactPoint::enforce() {
glm::vec3 pointA = _shapeA->getTranslation() + _offsetA;
glm::vec3 pointB = _shapeB->getTranslation() + _offsetB;
glm::vec3 penetration = pointA - pointB;
float pDotN = glm::dot(penetration, _normal);
bool constraintViolation = (pDotN > CONTACT_PENETRATION_ALLOWANCE);
// the contact point will be the average of the two points on the shapes
_contactPoint = _relativeMassA * pointA + _relativeMassB * pointB;
if (constraintViolation) {
for (int i = 0; i < _numPointsA; ++i) {
VerletPoint* point = _points[i];
glm::vec3 offset = _offsets[i];
// split delta into parallel and perpendicular components
glm::vec3 delta = _contactPoint + offset - point->_position;
glm::vec3 paraDelta = glm::dot(delta, _normal) * _normal;
glm::vec3 perpDelta = delta - paraDelta;
// use the relative sizes of the components to decide how much perpenducular delta to use
// perpendicular < parallel ==> static friction ==> perpFactor = 1.0
// perpendicular > parallel ==> dynamic friction ==> cap to length of paraDelta ==> perpFactor < 1.0
float paraLength = _relativeMassB * glm::length(paraDelta);
float perpLength = _relativeMassA * glm::length(perpDelta);
float perpFactor = (perpLength > paraLength && perpLength > EPSILON) ? (paraLength / perpLength) : 1.0f;
// recombine the two components to get the final delta
delta = paraDelta + perpFactor * perpDelta;
glm::vec3 targetPosition = point->_position + delta;
_distances[i] = glm::distance(_contactPoint, targetPosition);
point->_position += delta;
}
for (int i = _numPointsA; i < _numPoints; ++i) {
VerletPoint* point = _points[i];
glm::vec3 offset = _offsets[i];
// split delta into parallel and perpendicular components
glm::vec3 delta = _contactPoint + offset - point->_position;
glm::vec3 paraDelta = glm::dot(delta, _normal) * _normal;
glm::vec3 perpDelta = delta - paraDelta;
// use the relative sizes of the components to decide how much perpenducular delta to use
// perpendicular < parallel ==> static friction ==> perpFactor = 1.0
// perpendicular > parallel ==> dynamic friction ==> cap to length of paraDelta ==> perpFactor < 1.0
float paraLength = _relativeMassA * glm::length(paraDelta);
float perpLength = _relativeMassB * glm::length(perpDelta);
float perpFactor = (perpLength > paraLength && perpLength > EPSILON) ? (paraLength / perpLength) : 1.0f;
// recombine the two components to get the final delta
delta = paraDelta + perpFactor * perpDelta;
glm::vec3 targetPosition = point->_position + delta;
_distances[i] = glm::distance(_contactPoint, targetPosition);
point->_position += delta;
}
} else {
for (int i = 0; i < _numPoints; ++i) {
_distances[i] = glm::length(glm::length(_offsets[i]));
}
}
return 0.0f;
}
// virtual
void ContactPoint::applyFriction() {
// TODO: Andrew to re-implement this in a different way
/*
for (int i = 0; i < _numPoints; ++i) {
glm::vec3& position = _points[i]->_position;
// TODO: use a fast distance approximation
float newDistance = glm::distance(_contactPoint, position);
float constrainedDistance = _distances[i];
// NOTE: these "distance" constraints only push OUT, don't pull IN.
if (newDistance > EPSILON && newDistance < constrainedDistance) {
glm::vec3 direction = (_contactPoint - position) / newDistance;
glm::vec3 center = 0.5f * (_contactPoint + position);
_contactPoint = center + (0.5f * constrainedDistance) * direction;
position = center - (0.5f * constrainedDistance) * direction;
}
}
*/
}
void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) {
_lastFrame = frame;
// compute contact points on surface of each shape
glm::vec3 pointA = collision._contactPoint;
glm::vec3 pointB = pointA - collision._penetration;
// compute the normal (which points from A into B)
float pLength = glm::length(collision._penetration);
if (pLength > EPSILON) {
_normal = collision._penetration / pLength;
} else {
_normal = glm::vec3(0.0f);
}
if (collision._shapeA->getID() > collision._shapeB->getID()) {
// our _shapeA always has lower ID
_normal = - _normal;
pointA = pointB;
pointB = collision._contactPoint;
}
// bring the contact points inside the shapes to help maintain collision updates
pointA -= CONTACT_PENETRATION_ALLOWANCE * _normal;
pointB += CONTACT_PENETRATION_ALLOWANCE * _normal;
// compute relative offsets to per-shape contact points
_offsetA = pointA - collision._shapeA->getTranslation();
_offsetB = pointB - collision._shapeB->getTranslation();
// compute offsets for shapeA
assert(_offsets.size() == _numPoints);
for (int i = 0; i < _numPointsA; ++i) {
_offsets[i] = _points[i]->_position - pointA;
}
// compute offsets for shapeB
for (int i = _numPointsA; i < _numPoints; ++i) {
_offsets[i] = _points[i]->_position - pointB;
}
}

55
libraries/physics/src/ContactPoint.h
View file

@ -1,55 +0,0 @@
//
// ContactPoint.h
// libraries/physics/src
//
// Created by Andrew Meadows 2014.07.30
// 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_ContactPoint_h
#define hifi_ContactPoint_h
#include <QtGlobal>
#include <glm/glm.hpp>
#include <CollisionInfo.h>
#include "VerletPoint.h"
class Shape;
class ContactPoint {
public:
ContactPoint();
ContactPoint(const CollisionInfo& collision, quint32 frame);
virtual float enforce();
void applyFriction();
void updateContact(const CollisionInfo& collision, quint32 frame);
quint32 getLastFrame() const { return _lastFrame; }
Shape* getShapeA() const { return _shapeA; }
Shape* getShapeB() const { return _shapeB; }
protected:
quint32 _lastFrame; // frame count of last update
Shape* _shapeA;
Shape* _shapeB;
glm::vec3 _offsetA; // contact point relative to A's center
glm::vec3 _offsetB; // contact point relative to B's center
glm::vec3 _contactPoint; // a "virtual" point that is added to the simulation
float _relativeMassA; // massA / totalMass
float _relativeMassB; // massB / totalMass
int _numPointsA; // number of VerletPoints that belong to _shapeA
int _numPoints; // total number of VerletPoints
QVector<VerletPoint*> _points; // points that belong to colliding shapes
QVector<glm::vec3> _offsets; // offsets to _points from contactPoint
QVector<float> _distances; // distances to _points from contactPoint (during enforcement stage)
glm::vec3 _normal; // (points from A toward B)
};
#endif // hifi_ContactPoint_h

44
libraries/physics/src/DistanceConstraint.cpp
View file

@ -1,44 +0,0 @@
//
// DistanceConstraint.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2014.07.24
// 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
//
#include <SharedUtil.h> // for EPSILON
#include "DistanceConstraint.h"
#include "VerletPoint.h"
DistanceConstraint::DistanceConstraint(VerletPoint* startPoint, VerletPoint* endPoint) : _distance(-1.0f) {
_points[0] = startPoint;
_points[1] = endPoint;
_distance = glm::distance(_points[0]->_position, _points[1]->_position);
}
DistanceConstraint::DistanceConstraint(const DistanceConstraint& other) {
_distance = other._distance;
_points[0] = other._points[0];
_points[1] = other._points[1];
}
void DistanceConstraint::setDistance(float distance) {
_distance = fabsf(distance);
}
float DistanceConstraint::enforce() {
// TODO: use a fast distance approximation
float newDistance = glm::distance(_points[0]->_position, _points[1]->_position);
glm::vec3 direction(0.0f, 1.0f, 0.0f);
if (newDistance > EPSILON) {
direction = (_points[0]->_position - _points[1]->_position) / newDistance;
}
glm::vec3 center = 0.5f * (_points[0]->_position + _points[1]->_position);
_points[0]->_position = center + (0.5f * _distance) * direction;
_points[1]->_position = center - (0.5f * _distance) * direction;
return glm::abs(newDistance - _distance);
}

31
libraries/physics/src/DistanceConstraint.h
View file

@ -1,31 +0,0 @@
//
// DistanceConstraint.h
// libraries/physics/src
//
// Created by Andrew Meadows 2014.07.24
// 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_DistanceConstraint_h
#define hifi_DistanceConstraint_h
#include "Constraint.h"
class VerletPoint;
class DistanceConstraint : public Constraint {
public:
DistanceConstraint(VerletPoint* startPoint, VerletPoint* endPoint);
DistanceConstraint(const DistanceConstraint& other);
float enforce();
void setDistance(float distance);
float getDistance() const { return _distance; }
private:
float _distance;
VerletPoint* _points[2];
};
#endif // hifi_DistanceConstraint_h

35
libraries/physics/src/FixedConstraint.cpp
View file

@ -1,35 +0,0 @@
//
// FixedConstraint.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2014.07.24
// 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
//
#include "FixedConstraint.h"
#include "VerletPoint.h"
FixedConstraint::FixedConstraint(glm::vec3* anchor, VerletPoint* point) : _anchor(anchor), _point(point) {
assert(anchor);
assert(point);
}
float FixedConstraint::enforce() {
assert(_point != NULL);
_point->_position = *_anchor;
_point->_lastPosition = *_anchor;
return 0.0f;
}
void FixedConstraint::setAnchor(glm::vec3* anchor) {
assert(anchor);
_anchor = anchor;
}
void FixedConstraint::setPoint(VerletPoint* point) {
assert(point);
_point = point;
}

36
libraries/physics/src/FixedConstraint.h
View file

@ -1,36 +0,0 @@
//
// FixedConstraint.h
// libraries/physics/src
//
// Created by Andrew Meadows 2014.07.24
// 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_FixedConstraint_h
#define hifi_FixedConstraint_h
#include <glm/glm.hpp>
#include "Constraint.h"
class VerletPoint;
// FixedConstraint takes pointers to a glm::vec3 and a VerletPoint.
// The enforcement will copy the value of the vec3 into the VerletPoint.
class FixedConstraint : public Constraint {
public:
FixedConstraint(glm::vec3* anchor, VerletPoint* point);
~FixedConstraint() {}
float enforce();
void setAnchor(glm::vec3* anchor);
void setPoint(VerletPoint* point);
private:
glm::vec3* _anchor;
VerletPoint* _point;
};
#endif // hifi_FixedConstraint_h

32
libraries/physics/src/PhysicsEntity.cpp
View file

@ -11,7 +11,6 @@
#include "PhysicsEntity.h"
#include "PhysicsSimulation.h"
#include "PlaneShape.h"
#include "Shape.h"
#include "ShapeCollider.h"
@ -22,15 +21,10 @@ PhysicsEntity::PhysicsEntity() :
_rotation(),
_boundingRadius(0.0f),
_shapesAreDirty(true),
_enableShapes(false),
_simulation(NULL) {
_enableShapes(false) {
}
PhysicsEntity::~PhysicsEntity() {
if (_simulation) {
_simulation->removeEntity(this);
_simulation = NULL;
}
}
void PhysicsEntity::setTranslation(const glm::vec3& translation) {
@ -67,9 +61,6 @@ void PhysicsEntity::setEnableShapes(bool enable) {
}
void PhysicsEntity::clearShapes() {
if (_simulation) {
_simulation->removeShapes(this);
}
for (int i = 0; i < _shapes.size(); ++i) {
delete _shapes[i];
}
@ -178,24 +169,3 @@ bool PhysicsEntity::collisionsAreEnabled(int shapeIndexA, int shapeIndexB) const
}
return false;
}
void PhysicsEntity::disableCurrentSelfCollisions() {
CollisionList collisions(10);
int numShapes = _shapes.size();
for (int i = 0; i < numShapes; ++i) {
const Shape* shape = _shapes.at(i);
if (!shape) {
continue;
}
for (int j = i+1; j < numShapes; ++j) {
if (!collisionsAreEnabled(i, j)) {
continue;
}
const Shape* otherShape = _shapes.at(j);
if (otherShape && ShapeCollider::collideShapes(shape, otherShape, collisions)) {
disableCollisions(i, j);
collisions.clear();
}
}
}
}

14
libraries/physics/src/PhysicsEntity.h
View file

@ -22,11 +22,6 @@
#include <RayIntersectionInfo.h>
class Shape;
class PhysicsSimulation;
// PhysicsEntity is the base class for anything that owns one or more Shapes that collide in a
// PhysicsSimulation. Each CollisionInfo generated by a PhysicsSimulation has back pointers to the
// two Shapes involved, and those Shapes may (optionally) have valid back pointers to their PhysicsEntity.
class PhysicsEntity {
@ -51,8 +46,6 @@ public:
virtual void clearShapes();
const QVector<Shape*> getShapes() const { return _shapes; }
PhysicsSimulation* getSimulation() const { return _simulation; }
bool findRayIntersection(RayIntersectionInfo& intersection) const;
bool findCollisions(const QVector<const Shape*> shapes, CollisionList& collisions);
bool findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius, CollisionList& collisions);
@ -61,8 +54,6 @@ public:
void disableCollisions(int shapeIndexA, int shapeIndexB);
bool collisionsAreEnabled(int shapeIndexA, int shapeIndexB) const;
void disableCurrentSelfCollisions();
protected:
glm::vec3 _translation;
glm::quat _rotation;
@ -71,11 +62,6 @@ protected:
bool _enableShapes;
QVector<Shape*> _shapes;
QSet<int> _disabledCollisions;
private:
// PhysicsSimulation is a friend so that it can set the protected _simulation backpointer
friend class PhysicsSimulation;
PhysicsSimulation* _simulation;
};
#endif // hifi_PhysicsEntity_h

405
libraries/physics/src/PhysicsSimulation.cpp
View file

@ -1,405 +0,0 @@
//
// PhysicsSimulation.cpp
// libraries/physcis/src
//
// Created by Andrew Meadows 2014.06.06
// 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
//
#include <glm/glm.hpp>
#include <PerfStat.h>
#include <SharedUtil.h>
#include "PhysicsSimulation.h"
#include "PhysicsEntity.h"
#include "Ragdoll.h"
#include "Shape.h"
#include "ShapeCollider.h"
int MAX_DOLLS_PER_SIMULATION = 16;
int MAX_ENTITIES_PER_SIMULATION = 64;
int MAX_COLLISIONS_PER_SIMULATION = 256;
PhysicsSimulation::PhysicsSimulation() : _translation(0.0f), _frameCount(0), _entity(NULL), _ragdoll(NULL),
_collisions(MAX_COLLISIONS_PER_SIMULATION) {
}
PhysicsSimulation::~PhysicsSimulation() {
clear();
}
void PhysicsSimulation::clear() {
// entities have a backpointer to this simulator that must be cleaned up
int numEntities = _otherEntities.size();
for (int i = 0; i < numEntities; ++i) {
_otherEntities[i]->_simulation = NULL;
}
_otherEntities.clear();
if (_entity) {
_entity->_simulation = NULL;
}
// but Ragdolls do not
_ragdoll = NULL;
_otherRagdolls.clear();
// contacts have backpointers to shapes so we clear them
_contacts.clear();
}
void PhysicsSimulation::setRagdoll(Ragdoll* ragdoll) {
if (_ragdoll != ragdoll) {
if (_ragdoll) {
_ragdoll->_simulation = NULL;
}
_ragdoll = ragdoll;
if (_ragdoll) {
assert(!(_ragdoll->_simulation));
_ragdoll->_simulation = this;
}
}
}
void PhysicsSimulation::setEntity(PhysicsEntity* entity) {
if (_entity != entity) {
if (_entity) {
assert(_entity->_simulation == this);
_entity->_simulation = NULL;
}
_entity = entity;
if (_entity) {
assert(!(_entity->_simulation));
_entity->_simulation = this;
}
}
}
bool PhysicsSimulation::addEntity(PhysicsEntity* entity) {
if (!entity) {
return false;
}
if (entity->_simulation == this) {
int numEntities = _otherEntities.size();
for (int i = 0; i < numEntities; ++i) {
if (entity == _otherEntities.at(i)) {
// already in list
return true;
}
}
// belongs to some other simulation
return false;
}
int numEntities = _otherEntities.size();
if (numEntities > MAX_ENTITIES_PER_SIMULATION) {
// list is full
return false;
}
// add to list
assert(!(entity->_simulation));
entity->_simulation = this;
_otherEntities.push_back(entity);
return true;
}
void PhysicsSimulation::removeEntity(PhysicsEntity* entity) {
if (!entity || !entity->_simulation || !(entity->_simulation == this)) {
return;
}
removeShapes(entity);
int numEntities = _otherEntities.size();
for (int i = 0; i < numEntities; ++i) {
if (entity == _otherEntities.at(i)) {
if (i == numEntities - 1) {
// remove it
_otherEntities.pop_back();
} else {
// swap the last for this one
PhysicsEntity* lastEntity = _otherEntities[numEntities - 1];
_otherEntities.pop_back();
_otherEntities[i] = lastEntity;
}
entity->_simulation = NULL;
break;
}
}
}
void PhysicsSimulation::removeShapes(const PhysicsEntity* entity) {
// remove data structures with pointers to entity's shapes
QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
while (itr != _contacts.end()) {
if (entity == itr.value().getShapeA()->getEntity() || entity == itr.value().getShapeB()->getEntity()) {
itr = _contacts.erase(itr);
} else {
++itr;
}
}
}
void PhysicsSimulation::removeShape(const Shape* shape) {
// remove data structures with pointers to shape
QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
while (itr != _contacts.end()) {
if (shape == itr.value().getShapeA() || shape == itr.value().getShapeB()) {
itr = _contacts.erase(itr);
} else {
++itr;
}
}
}
const float OTHER_RAGDOLL_MASS_SCALE = 10.0f;
bool PhysicsSimulation::addRagdoll(Ragdoll* doll) {
if (!doll) {
return false;
}
int numDolls = _otherRagdolls.size();
if (numDolls > MAX_DOLLS_PER_SIMULATION) {
// list is full
return false;
}
if (doll->_simulation == this) {
for (int i = 0; i < numDolls; ++i) {
if (doll == _otherRagdolls[i]) {
// already in list
return true;
}
}
}
// add to list
assert(!(doll->_simulation));
doll->_simulation = this;
_otherRagdolls.push_back(doll);
// set the massScale of otherRagdolls artificially high
doll->setMassScale(OTHER_RAGDOLL_MASS_SCALE);
return true;
}
void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
if (!doll || doll->_simulation != this) {
return;
}
int numDolls = _otherRagdolls.size();
for (int i = 0; i < numDolls; ++i) {
if (doll == _otherRagdolls[i]) {
if (i == numDolls - 1) {
// remove it
_otherRagdolls.pop_back();
} else {
// swap the last for this one
Ragdoll* lastDoll = _otherRagdolls[numDolls - 1];
_otherRagdolls.pop_back();
_otherRagdolls[i] = lastDoll;
}
doll->_simulation = NULL;
doll->setMassScale(1.0f);
break;
}
}
}
void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) {
++_frameCount;
quint64 now = usecTimestampNow();
quint64 startTime = now;
quint64 expiry = startTime + maxUsec;
integrate(deltaTime);
enforceContacts();
int numDolls = _otherRagdolls.size();
{
PerformanceTimer perfTimer("enforce");
if (_ragdoll) {
_ragdoll->enforceConstraints();
}
for (int i = 0; i < numDolls; ++i) {
_otherRagdolls[i]->enforceConstraints();
}
}
bool collidedWithOtherRagdoll = false;
int iterations = 0;
float error = 0.0f;
do {
collidedWithOtherRagdoll = computeCollisions() || collidedWithOtherRagdoll;
updateContacts();
resolveCollisions();
{ // enforce constraints
PerformanceTimer perfTimer("enforce");
if (_ragdoll) {
error = _ragdoll->enforceConstraints();
}
for (int i = 0; i < numDolls; ++i) {
error = glm::max(error, _otherRagdolls[i]->enforceConstraints());
}
}
applyContactFriction();
++iterations;
now = usecTimestampNow();
} while (_collisions.size() != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry));
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) {
_otherRagdolls[i]->removeRootOffset(false);
}
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);
}
if (_ragdoll) {
_ragdoll->stepForward(deltaTime);
}
int numDolls = _otherRagdolls.size();
for (int i = 0; i < numDolls; ++i) {
_otherRagdolls[i]->stepForward(deltaTime);
}
}
bool PhysicsSimulation::computeCollisions() {
PerformanceTimer perfTimer("collide");
_collisions.clear();
const QVector<Shape*> shapes = _entity->getShapes();
int numShapes = shapes.size();
// collide main ragdoll with self
for (int i = 0; i < numShapes; ++i) {
const Shape* shape = shapes.at(i);
if (!shape) {
continue;
}
for (int j = i+1; j < numShapes; ++j) {
const Shape* otherShape = shapes.at(j);
if (otherShape && _entity->collisionsAreEnabled(i, j)) {
ShapeCollider::collideShapes(shape, otherShape, _collisions);
}
}
}
// collide main ragdoll with others
bool otherCollisions = false;
int numEntities = _otherEntities.size();
for (int i = 0; i < numEntities; ++i) {
const QVector<Shape*> otherShapes = _otherEntities.at(i)->getShapes();
otherCollisions = ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions) || otherCollisions;
}
return otherCollisions;
}
void PhysicsSimulation::resolveCollisions() {
PerformanceTimer perfTimer("resolve");
// walk all collisions, accumulate movement on shapes, and build a list of affected shapes
QSet<Shape*> shapes;
int numCollisions = _collisions.size();
for (int i = 0; i < numCollisions; ++i) {
CollisionInfo* collision = _collisions.getCollision(i);
collision->apply();
// there is always a shapeA
shapes.insert(collision->getShapeA());
// but need to check for valid shapeB
if (collision->_shapeB) {
shapes.insert(collision->getShapeB());
}
}
// walk all affected shapes and apply accumulated movement
QSet<Shape*>::const_iterator shapeItr = shapes.constBegin();
while (shapeItr != shapes.constEnd()) {
(*shapeItr)->applyAccumulatedDelta();
++shapeItr;
}
}
void PhysicsSimulation::enforceContacts() {
PerformanceTimer perfTimer("contacts");
QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
while (itr != _contacts.end()) {
itr.value().enforce();
++itr;
}
}
void PhysicsSimulation::applyContactFriction() {
PerformanceTimer perfTimer("contacts");
QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
while (itr != _contacts.end()) {
itr.value().applyFriction();
++itr;
}
}
void PhysicsSimulation::updateContacts() {
PerformanceTimer perfTimer("contacts");
int numCollisions = _collisions.size();
for (int i = 0; i < numCollisions; ++i) {
CollisionInfo* collision = _collisions.getCollision(i);
quint64 key = collision->getShapePairKey();
if (key == 0) {
continue;
}
QMap<quint64, ContactPoint>::iterator itr = _contacts.find(key);
if (itr == _contacts.end()) {
_contacts.insert(key, ContactPoint(*collision, _frameCount));
} else {
itr.value().updateContact(*collision, _frameCount);
}
}
}
const quint32 MAX_CONTACT_FRAME_LIFETIME = 2;
void PhysicsSimulation::pruneContacts() {
QMap<quint64, ContactPoint>::iterator itr = _contacts.begin();
while (itr != _contacts.end()) {
if (_frameCount - itr.value().getLastFrame() > MAX_CONTACT_FRAME_LIFETIME) {
itr = _contacts.erase(itr);
} else {
++itr;
}
}
}

92
libraries/physics/src/PhysicsSimulation.h
View file

@ -1,92 +0,0 @@
//
// PhysicsSimulation.h
// libraries/physcis/src
//
// Created by Andrew Meadows 2014.06.06
// 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_PhysicsSimulation_h
#define hifi_PhysicsSimulation_h
#include <QtGlobal>
#include <QMap>
#include <QVector>
#include "CollisionInfo.h"
#include "ContactPoint.h"
#include "RayIntersectionInfo.h"
class PhysicsEntity;
class Ragdoll;
class PhysicsSimulation {
public:
PhysicsSimulation();
~PhysicsSimulation();
void clear();
void setTranslation(const glm::vec3& translation) { _translation = translation; }
const glm::vec3& getTranslation() const { return _translation; }
void setRagdoll(Ragdoll* ragdoll);
void setEntity(PhysicsEntity* entity);
/// \return true if entity was added to or is already in the list
bool addEntity(PhysicsEntity* entity);
void removeEntity(PhysicsEntity* entity);
void removeShapes(const PhysicsEntity* entity);
void removeShape(const Shape* shape);
/// \return true if doll was added to or is already in the list
bool addRagdoll(Ragdoll* doll);
void removeRagdoll(Ragdoll* doll);
/// \param minError constraint motion below this value is considered "close enough"
/// \param maxIterations max number of iterations before giving up
/// \param maxUsec max number of usec to spend enforcing constraints
/// \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;
void setupAvatarCollision();
protected:
void integrate(float deltaTime);
/// \return true if main ragdoll collides with other avatar
bool computeCollisions();
void resolveCollisions();
void enforceContacts();
void applyContactFriction();
void updateContacts();
void pruneContacts();
private:
glm::vec3 _translation; // origin of simulation in world-frame
quint32 _frameCount;
PhysicsEntity* _entity;
Ragdoll* _ragdoll;
QVector<Ragdoll*> _otherRagdolls;
QVector<PhysicsEntity*> _otherEntities;
CollisionList _collisions;
QMap<quint64, ContactPoint> _contacts;
};
#endif // hifi_PhysicsSimulation_h

146
libraries/physics/src/Ragdoll.cpp
View file

@ -1,146 +0,0 @@
//
// Ragdoll.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2014.05.30
// 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
//
#include <glm/gtx/norm.hpp>
#include <SharedUtil.h> // for EPSILON
#include "Ragdoll.h"
#include "Constraint.h"
#include "DistanceConstraint.h"
#include "FixedConstraint.h"
#include "PhysicsSimulation.h"
Ragdoll::Ragdoll() : _massScale(1.0f), _translation(0.0f), _translationInSimulationFrame(0.0f),
_rootIndex(0), _accumulatedMovement(0.0f), _simulation(NULL) {
}
Ragdoll::~Ragdoll() {
clearConstraintsAndPoints();
if (_simulation) {
_simulation->removeRagdoll(this);
}
}
void Ragdoll::stepForward(float deltaTime) {
if (_simulation) {
updateSimulationTransforms(_translation - _simulation->getTranslation(), _rotation);
}
int numPoints = _points.size();
for (int i = _rootIndex; i < numPoints; ++i) {
_points[i].integrateForward();
}
}
void Ragdoll::clearConstraintsAndPoints() {
int numConstraints = _boneConstraints.size();
for (int i = 0; i < numConstraints; ++i) {
delete _boneConstraints[i];
}
_boneConstraints.clear();
numConstraints = _fixedConstraints.size();
for (int i = 0; i < numConstraints; ++i) {
delete _fixedConstraints[i];
}
_fixedConstraints.clear();
_points.clear();
}
float Ragdoll::enforceConstraints() {
float maxDistance = 0.0f;
// enforce the bone constraints first
int numConstraints = _boneConstraints.size();
for (int i = 0; i < numConstraints; ++i) {
maxDistance = glm::max(maxDistance, _boneConstraints[i]->enforce());
}
// enforce FixedConstraints second
numConstraints = _fixedConstraints.size();
for (int i = 0; i < _fixedConstraints.size(); ++i) {
maxDistance = glm::max(maxDistance, _fixedConstraints[i]->enforce());
}
return maxDistance;
}
void Ragdoll::initTransform() {
_translation = glm::vec3(0.0f);
_rotation = glm::quat();
_translationInSimulationFrame = glm::vec3(0.0f);
_rotationInSimulationFrame = glm::quat();
}
void Ragdoll::setTransform(const glm::vec3& translation, const glm::quat& rotation) {
if (translation != _translation) {
_translation = translation;
}
_rotation = rotation;
}
void Ragdoll::updateSimulationTransforms(const glm::vec3& translation, const glm::quat& rotation) {
const float EPSILON2 = EPSILON * EPSILON;
if (glm::distance2(translation, _translationInSimulationFrame) < EPSILON2 &&
glm::abs(1.0f - glm::abs(glm::dot(rotation, _rotationInSimulationFrame))) < EPSILON2) {
// nothing to do
return;
}
// compute linear and angular deltas
glm::vec3 deltaPosition = translation - _translationInSimulationFrame;
glm::quat deltaRotation = rotation * glm::inverse(_rotationInSimulationFrame);
// apply the deltas to all ragdollPoints
int numPoints = _points.size();
for (int i = _rootIndex; i < numPoints; ++i) {
_points[i].move(deltaPosition, deltaRotation, _translationInSimulationFrame);
}
// remember the current transform
_translationInSimulationFrame = translation;
_rotationInSimulationFrame = rotation;
}
void Ragdoll::setMassScale(float scale) {
const float MIN_SCALE = 1.0e-2f;
const float MAX_SCALE = 1.0e6f;
scale = glm::clamp(glm::abs(scale), MIN_SCALE, MAX_SCALE);
if (scale != _massScale) {
float rescale = scale / _massScale;
int numPoints = _points.size();
for (int i = _rootIndex; i < numPoints; ++i) {
_points[i].setMass(rescale * _points[i].getMass());
}
_massScale = scale;
}
}
void Ragdoll::removeRootOffset(bool accumulateMovement) {
const int numPoints = _points.size();
if (numPoints > 0) {
// shift all points so that the root aligns with the the ragdoll's position in the simulation
glm::vec3 offset = _translationInSimulationFrame - _points[_rootIndex]._position;
float offsetLength = glm::length(offset);
if (offsetLength > EPSILON) {
for (int i = _rootIndex; i < numPoints; ++i) {
_points[i].shift(offset);
}
const float MIN_ROOT_OFFSET = 0.02f;
if (accumulateMovement && offsetLength > MIN_ROOT_OFFSET) {
_accumulatedMovement -= (1.0f - MIN_ROOT_OFFSET / offsetLength) * offset;
}
}
}
}
glm::vec3 Ragdoll::getAndClearAccumulatedMovement() {
glm::vec3 movement = _accumulatedMovement;
_accumulatedMovement = glm::vec3(0.0f);
return movement;
}

91
libraries/physics/src/Ragdoll.h
View file

@ -1,91 +0,0 @@
//
// Ragdoll.h
// libraries/physics/src
//
// Created by Andrew Meadows 2014.05.30
// 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_Ragdoll_h
#define hifi_Ragdoll_h
#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>
#include <QVector>
#include "VerletPoint.h"
//#include "PhysicsSimulation.h"
class DistanceConstraint;
class FixedConstraint;
class PhysicsSimulation;
// TODO: don't derive SkeletonModel from Ragdoll so we can clean up the Ragdoll API
// (==> won't need to worry about namespace conflicts between Entity and Ragdoll).
class Ragdoll {
public:
Ragdoll();
virtual ~Ragdoll();
virtual void stepForward(float deltaTime);
/// \return max distance of point movement
float enforceConstraints();
// both const and non-const getPoints()
const QVector<VerletPoint>& getPoints() const { return _points; }
QVector<VerletPoint>& getPoints() { return _points; }
void initTransform();
/// set the translation and rotation of the Ragdoll and adjust all VerletPoints.
void setTransform(const glm::vec3& translation, const glm::quat& rotation);
const glm::vec3& getTranslationInSimulationFrame() const { return _translationInSimulationFrame; }
void setMassScale(float scale);
float getMassScale() const { return _massScale; }
// the ragdoll's rootIndex (within a Model's joints) is not always zero so must be settable
void setRootIndex(int index) { _rootIndex = index; }
int getRootIndex() const { return _rootIndex; }
void clearConstraintsAndPoints();
virtual void initPoints() = 0;
virtual void buildConstraints() = 0;
void removeRootOffset(bool accumulateMovement);
glm::vec3 getAndClearAccumulatedMovement();
protected:
float _massScale;
glm::vec3 _translation; // world-frame
glm::quat _rotation; // world-frame
glm::vec3 _translationInSimulationFrame;
glm::quat _rotationInSimulationFrame;
int _rootIndex;
QVector<VerletPoint> _points;
QVector<DistanceConstraint*> _boneConstraints;
QVector<FixedConstraint*> _fixedConstraints;
// The collisions are typically done in a simulation frame that is slaved to the center of one of the Ragdolls.
// To allow the Ragdoll to provide feedback of its own displacement we store it in _accumulatedMovement.
// The owner of the Ragdoll can harvest this displacement to update the rest of the object positions in the simulation.
glm::vec3 _accumulatedMovement;
private:
void updateSimulationTransforms(const glm::vec3& translation, const glm::quat& rotation);
friend class PhysicsSimulation;
PhysicsSimulation* _simulation;
};
#endif // hifi_Ragdoll_h

170
libraries/physics/src/VerletCapsuleShape.cpp
View file

@ -1,170 +0,0 @@
//
// VerletCapsuleShape.cpp
// libraries/physics/src
//
// Created by Andrew Meadows on 2014.06.16
// 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
//
#include <SharedUtil.h>
#include "VerletCapsuleShape.h"
#include "VerletPoint.h"
VerletCapsuleShape::VerletCapsuleShape(VerletPoint* startPoint, VerletPoint* endPoint) :
CapsuleShape(), _startPoint(startPoint), _endPoint(endPoint), _startLagrangeCoef(0.5f), _endLagrangeCoef(0.5f) {
assert(startPoint);
assert(endPoint);
_halfHeight = 0.5f * glm::distance(_startPoint->_position, _endPoint->_position);
updateBoundingRadius();
}
VerletCapsuleShape::VerletCapsuleShape(float radius, VerletPoint* startPoint, VerletPoint* endPoint) :
CapsuleShape(radius, 1.0f), _startPoint(startPoint), _endPoint(endPoint),
_startLagrangeCoef(0.5f), _endLagrangeCoef(0.5f) {
assert(startPoint);
assert(endPoint);
_halfHeight = 0.5f * glm::distance(_startPoint->_position, _endPoint->_position);
updateBoundingRadius();
}
const glm::quat& VerletCapsuleShape::getRotation() const {
// NOTE: The "rotation" of this shape must be computed on the fly,
// which makes this method MUCH more more expensive than you might expect.
glm::vec3 axis;
computeNormalizedAxis(axis);
VerletCapsuleShape* thisCapsule = const_cast<VerletCapsuleShape*>(this);
thisCapsule->_rotation = computeNewRotation(axis);
return _rotation;
}
void VerletCapsuleShape::setRotation(const glm::quat& rotation) {
// NOTE: this method will update the verlet points, which is probably not
// what you want to do. Only call this method if you know what you're doing.
// update points such that they have the same center but a different axis
glm::vec3 center = getTranslation();
float halfHeight = getHalfHeight();
glm::vec3 axis = rotation * DEFAULT_CAPSULE_AXIS;
_startPoint->_position = center - halfHeight * axis;
_endPoint->_position = center + halfHeight * axis;
}
void VerletCapsuleShape::setTranslation(const glm::vec3& position) {
// NOTE: this method will update the verlet points, which is probably not
// what you want to do. Only call this method if you know what you're doing.
// update the points such that their center is at position
glm::vec3 movement = position - getTranslation();
_startPoint->_position += movement;
_endPoint->_position += movement;
}
const glm::vec3& VerletCapsuleShape::getTranslation() const {
// the "translation" of this shape must be computed on the fly
VerletCapsuleShape* thisCapsule = const_cast<VerletCapsuleShape*>(this);
thisCapsule->_translation = 0.5f * (_startPoint->_position + _endPoint->_position);
return _translation;
}
float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) {
glm::vec3 startLeg = _startPoint->_position - contactPoint;
glm::vec3 endLeg = _endPoint->_position - contactPoint;
// TODO: use fast approximate distance calculations here
float startLength = glm::length(startLeg);
float endlength = glm::length(endLeg);
// The raw coefficient is proportional to the other leg's length multiplied by the dot-product
// of the penetration and this leg direction. We don't worry about the common penetration length
// because it is normalized out later.
float startCoef = glm::abs(glm::dot(startLeg, penetration)) * endlength / (startLength + EPSILON);
float endCoef = glm::abs(glm::dot(endLeg, penetration)) * startLength / (endlength + EPSILON);
float maxCoef = glm::max(startCoef, endCoef);
if (maxCoef > EPSILON) {
// One of these coeficients will be 1.0, the other will be less -->
// one endpoint will move the full amount while the other will move less.
_startLagrangeCoef = startCoef / maxCoef;
_endLagrangeCoef = endCoef / maxCoef;
} else {
// The coefficients are the same --> the collision will move both equally
// as if the contact were at the center of mass.
_startLagrangeCoef = 1.0f;
_endLagrangeCoef = 1.0f;
}
// the effective mass is the weighted sum of the two endpoints
return _startLagrangeCoef * _startPoint->getMass() + _endLagrangeCoef * _endPoint->getMass();
}
void VerletCapsuleShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) {
assert(!glm::isnan(relativeMassFactor));
_startPoint->accumulateDelta((relativeMassFactor * _startLagrangeCoef) * penetration);
_endPoint->accumulateDelta((relativeMassFactor * _endLagrangeCoef) * penetration);
}
void VerletCapsuleShape::applyAccumulatedDelta() {
_startPoint->applyAccumulatedDelta();
_endPoint->applyAccumulatedDelta();
}
void VerletCapsuleShape::getVerletPoints(QVector<VerletPoint*>& points) {
points.push_back(_startPoint);
points.push_back(_endPoint);
}
// virtual
float VerletCapsuleShape::getHalfHeight() const {
return 0.5f * glm::distance(_startPoint->_position, _endPoint->_position);
}
// virtual
void VerletCapsuleShape::getStartPoint(glm::vec3& startPoint) const {
startPoint = _startPoint->_position;
}
// virtual
void VerletCapsuleShape::getEndPoint(glm::vec3& endPoint) const {
endPoint = _endPoint->_position;
}
// virtual
void VerletCapsuleShape::computeNormalizedAxis(glm::vec3& axis) const {
glm::vec3 unormalizedAxis = _endPoint->_position - _startPoint->_position;
float fullLength = glm::length(unormalizedAxis);
if (fullLength > EPSILON) {
axis = unormalizedAxis / fullLength;
} else {
// the axis is meaningless, but we fill it with a normalized direction
// just in case the calling context assumes it really is normalized.
axis = glm::vec3(0.0f, 1.0f, 0.0f);
}
}
// virtual
void VerletCapsuleShape::setHalfHeight(float halfHeight) {
// push points along axis so they are 2*halfHeight apart
glm::vec3 center = getTranslation();
glm::vec3 axis;
computeNormalizedAxis(axis);
_startPoint->_position = center - halfHeight * axis;
_endPoint->_position = center + halfHeight * axis;
_boundingRadius = _radius + halfHeight;
}
// virtual
void VerletCapsuleShape::setRadiusAndHalfHeight(float radius, float halfHeight) {
_radius = radius;
setHalfHeight(halfHeight);
}
// virtual
void VerletCapsuleShape::setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint) {
_startPoint->_position = startPoint;
_endPoint->_position = endPoint;
updateBoundingRadius();
}

84
libraries/physics/src/VerletCapsuleShape.h
View file

@ -1,84 +0,0 @@
//
// VerletCapsuleShape.h
// libraries/physics/src
//
// Created by Andrew Meadows on 2014.06.16
// 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_VerletCapsuleShape_h
#define hifi_VerletCapsuleShape_h
#include "CapsuleShape.h"
// The VerletCapsuleShape is similar to a regular CapsuleShape, except it keeps a pointer
// to its endpoints which are owned by some other data structure (a verlet simulation system).
// This makes it easier for the points to be moved around by constraints in the system
// as well as collisions with the shape, however it has some drawbacks:
//
// (1) The Shape::_translation and ::_rotation data members are not used (wasted)
//
// (2) A VerletShape doesn't own the points that it uses, so you must be careful not to
// leave dangling pointers around.
//
// (3) Some const methods of VerletCapsuleShape are much more expensive than you might think.
// For example getHalfHeight() and setHalfHeight() methods must do extra computation. In
// particular setRotation() is significantly more expensive than for the CapsuleShape.
// Not too expensive to use when setting up shapes, but you woudln't want to use it deep
// down in a hot simulation loop, such as when processing collision results. Best to
// just let the verlet simulation do its thing and not try to constantly force a rotation.
class VerletPoint;
class VerletCapsuleShape : public CapsuleShape {
public:
VerletCapsuleShape(VerletPoint* startPoint, VerletPoint* endPoint);
VerletCapsuleShape(float radius, VerletPoint* startPoint, VerletPoint* endPoint);
// virtual overrides from Shape
const glm::quat& getRotation() const;
void setRotation(const glm::quat& rotation);
void setTranslation(const glm::vec3& position);
const glm::vec3& getTranslation() const;
float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint);
void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration);
void applyAccumulatedDelta();
virtual void getVerletPoints(QVector<VerletPoint*>& points);
//float getRadius() const { return _radius; }
virtual float getHalfHeight() const;
/// \param[out] startPoint is the center of start cap
void getStartPoint(glm::vec3& startPoint) const;
/// \param[out] endPoint is the center of the end cap
void getEndPoint(glm::vec3& endPoint) const;
/// \param[out] axis is a normalized vector that points from start to end
void computeNormalizedAxis(glm::vec3& axis) const;
//void setRadius(float radius);
void setHalfHeight(float halfHeight);
void setRadiusAndHalfHeight(float radius, float halfHeight);
void setEndPoints(const glm::vec3& startPoint, const glm::vec3& endPoint);
//void assignEndPoints(glm::vec3* startPoint, glm::vec3* endPoint);
protected:
// NOTE: VerletCapsuleShape does NOT own the data in its points.
VerletPoint* _startPoint;
VerletPoint* _endPoint;
// The LagrangeCoef's are numerical weights for distributing collision movement
// between the relevant VerletPoints associated with this shape. They are functions
// of the movement parameters and are computed (and cached) in computeEffectiveMass()
// and then used in the subsequent accumulateDelta().
float _startLagrangeCoef;
float _endLagrangeCoef;
};
#endif // hifi_VerletCapsuleShape_h

54
libraries/physics/src/VerletPoint.cpp
View file

@ -1,54 +0,0 @@
//
// VerletPoint.cpp
// libraries/physics/src
//
// Created by Andrew Meadows 2014.07.24
// 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
//
#include "VerletPoint.h"
const float INTEGRATION_FRICTION_FACTOR = 0.6f;
void VerletPoint::integrateForward() {
glm::vec3 oldPosition = _position;
_position += INTEGRATION_FRICTION_FACTOR * (_position - _lastPosition);
_lastPosition = oldPosition;
}
void VerletPoint::accumulateDelta(const glm::vec3& delta) {
_accumulatedDelta += delta;
++_numDeltas;
}
void VerletPoint::applyAccumulatedDelta() {
if (_numDeltas > 0) {
_position += _accumulatedDelta / (float)_numDeltas;
_accumulatedDelta = glm::vec3(0.0f);
_numDeltas = 0;
}
}
void VerletPoint::move(const glm::vec3& deltaPosition, const glm::quat& deltaRotation, const glm::vec3& oldPivot) {
glm::vec3 arm = _position - oldPivot;
_position += deltaPosition + (deltaRotation * arm - arm);
arm = _lastPosition - oldPivot;
_lastPosition += deltaPosition + (deltaRotation * arm - arm);
}
void VerletPoint::shift(const glm::vec3& deltaPosition) {
_position += deltaPosition;
_lastPosition += deltaPosition;
}
void VerletPoint::setMass(float mass) {
const float MIN_MASS = 1.0e-6f;
const float MAX_MASS = 1.0e18f;
if (glm::isnan(mass)) {
mass = MIN_MASS;
}
_mass = glm::clamp(glm::abs(mass), MIN_MASS, MAX_MASS);
}

42
libraries/physics/src/VerletPoint.h
View file

@ -1,42 +0,0 @@
//
// VerletPoint.h
// libraries/physics/src
//
// Created by Andrew Meadows 2014.07.24
// 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_VerletPoint_h
#define hifi_VerletPoint_h
#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
class VerletPoint {
public:
VerletPoint() : _position(0.0f), _lastPosition(0.0f), _mass(1.0f), _accumulatedDelta(0.0f), _numDeltas(0) {}
void initPosition(const glm::vec3& position) { _position = position; _lastPosition = position; }
void integrateForward();
void accumulateDelta(const glm::vec3& delta);
void applyAccumulatedDelta();
void move(const glm::vec3& deltaPosition, const glm::quat& deltaRotation, const glm::vec3& oldPivot);
void shift(const glm::vec3& deltaPosition);
void setMass(float mass);
float getMass() const { return _mass; }
glm::vec3 _position;
glm::vec3 _lastPosition;
private:
float _mass;
glm::vec3 _accumulatedDelta;
int _numDeltas;
};
#endif // hifi_VerletPoint_h

54
libraries/physics/src/VerletSphereShape.cpp
View file

@ -1,54 +0,0 @@
//
// VerletSphereShape.cpp
// libraries/physics/src
//
// Created by Andrew Meadows on 2014.06.16
// 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
//
#include "VerletSphereShape.h"
#include "Ragdoll.h" // for VerletPoint
VerletSphereShape::VerletSphereShape(VerletPoint* centerPoint) : SphereShape() {
assert(centerPoint);
_point = centerPoint;
}
VerletSphereShape::VerletSphereShape(float radius, VerletPoint* centerPoint) : SphereShape(radius) {
assert(centerPoint);
_point = centerPoint;
}
// virtual from Shape class
void VerletSphereShape::setTranslation(const glm::vec3& position) {
_point->_position = position;
_point->_lastPosition = position;
}
// virtual from Shape class
const glm::vec3& VerletSphereShape::getTranslation() const {
return _point->_position;
}
// virtual
float VerletSphereShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) {
return _point->getMass();
}
// virtual
void VerletSphereShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) {
_point->accumulateDelta(relativeMassFactor * penetration);
}
// virtual
void VerletSphereShape::applyAccumulatedDelta() {
_point->applyAccumulatedDelta();
}
void VerletSphereShape::getVerletPoints(QVector<VerletPoint*>& points) {
points.push_back(_point);
}

49
libraries/physics/src/VerletSphereShape.h
View file

@ -1,49 +0,0 @@
//
// VerletSphereShape.h
// libraries/physics/src
//
// Created by Andrew Meadows on 2014.06.16
// 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_VerletSphereShape_h
#define hifi_VerletSphereShape_h
#include "SphereShape.h"
// The VerletSphereShape is similar to a regular SphereShape, except it keeps a pointer
// to its center which is owned by some other data structure (a verlet simulation system).
// This makes it easier for the points to be moved around by constraints in the system
// as well as collisions with the shape, however it has some drawbacks:
//
// (1) The Shape::_translation data member is not used (wasted)
//
// (2) A VerletShape doesn't own the points that it uses, so you must be careful not to
// leave dangling pointers around.
class VerletPoint;
class VerletSphereShape : public SphereShape {
public:
VerletSphereShape(VerletPoint* point);
VerletSphereShape(float radius, VerletPoint* centerPoint);
// virtual overrides from Shape
void setTranslation(const glm::vec3& position);
const glm::vec3& getTranslation() const;
float computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint);
void accumulateDelta(float relativeMassFactor, const glm::vec3& penetration);
void applyAccumulatedDelta();
void getVerletPoints(QVector<VerletPoint*>& points);
protected:
// NOTE: VerletSphereShape does NOT own its _point
VerletPoint* _point;
};
#endif // hifi_VerletSphereShape_h

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

@ -21,7 +21,6 @@
#include "RayIntersectionInfo.h"
class PhysicsEntity;
class VerletPoint;
const float MAX_SHAPE_MASS = 1.0e18f; // something less than sqrt(FLT_MAX)
@ -87,8 +86,6 @@ public:
/// \return volume of shape in cubic meters
virtual float getVolume() const { return 1.0; }
virtual void getVerletPoints(QVector<VerletPoint*>& points) {}
virtual QDebug& dumpToDebug(QDebug& debugConext) const;
protected:

766
tests/physics/src/VerletShapeTests.cpp
View file

@ -1,766 +0,0 @@
//
// VerletShapeTests.cpp
// tests/physics/src
//
// Created by Andrew Meadows on 02/21/2014.
// 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
//
//#include <stdio.h>
#include <iostream>
#include <math.h>
#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>
#include <CollisionInfo.h>
#include <Ragdoll.h> // for VerletPoint
#include <ShapeCollider.h>
#include <SharedUtil.h>
#include <VerletCapsuleShape.h>
#include <VerletSphereShape.h>
#include <StreamUtils.h>
#include "VerletShapeTests.h"
const glm::vec3 origin(0.0f);
static const glm::vec3 xAxis(1.0f, 0.0f, 0.0f);
static const glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
static const glm::vec3 zAxis(0.0f, 0.0f, 1.0f);
void VerletShapeTests::setSpherePosition() {
float radius = 1.0f;
glm::vec3 offset(1.23f, 4.56f, 7.89f);
VerletPoint point;
VerletSphereShape sphere(radius, &point);
point._position = glm::vec3(0.0f);
float d = glm::distance(glm::vec3(0.0f), sphere.getTranslation());
if (d != 0.0f) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at origin" << std::endl;
}
point._position = offset;
d = glm::distance(glm::vec3(0.0f), sphere.getTranslation());
if (d != glm::length(offset)) {
std::cout << __FILE__ << ":" << __LINE__ << " ERROR: sphere should be at offset" << std::endl;
}
}
void VerletShapeTests::sphereMissesSphere() {
// non-overlapping spheres of unequal size
float radiusA = 7.0f;
float radiusB = 3.0f;
float alpha = 1.2f;
float beta = 1.3f;
glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
float offsetDistance = alpha * radiusA + beta * radiusB;
// create points for the sphere centers
VerletPoint points[2];
// give pointers to the spheres
VerletSphereShape sphereA(radiusA, (points + 0));
VerletSphereShape sphereB(radiusB, (points + 1));
// set the positions of the spheres by slamming the points directly
points[0]._position = origin;
points[1]._position = offsetDistance * offsetDirection;
CollisionList collisions(16);
// collide A to B...
{
bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions);
if (touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should NOT touch" << std::endl;
}
}
// collide B to A...
{
bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions);
if (touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should NOT touch" << std::endl;
}
}
// also test shapeShape
{
bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions);
if (touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should NOT touch" << std::endl;
}
}
if (collisions.size() > 0) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected empty collision list but size is " << collisions.size() << std::endl;
}
}
void VerletShapeTests::sphereTouchesSphere() {
// overlapping spheres of unequal size
float radiusA = 7.0f;
float radiusB = 3.0f;
float alpha = 0.2f;
float beta = 0.3f;
glm::vec3 offsetDirection = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f));
float offsetDistance = alpha * radiusA + beta * radiusB;
float expectedPenetrationDistance = (1.0f - alpha) * radiusA + (1.0f - beta) * radiusB;
glm::vec3 expectedPenetration = expectedPenetrationDistance * offsetDirection;
// create two points for the sphere centers
VerletPoint points[2];
// give pointers to the spheres
VerletSphereShape sphereA(radiusA, points+0);
VerletSphereShape sphereB(radiusB, points+1);
// set the positions of the spheres by slamming the points directly
points[0]._position = origin;
points[1]._position = offsetDistance * offsetDirection;
CollisionList collisions(16);
int numCollisions = 0;
// collide A to B...
{
bool touching = ShapeCollider::collideShapes(&sphereA, &sphereB, collisions);
if (!touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should touch" << std::endl;
} else {
++numCollisions;
}
// verify state of collisions
if (numCollisions != collisions.size()) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected collisions size of " << numCollisions << " but actual size is " << collisions.size()
<< std::endl;
}
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
if (!collision) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: null collision" << std::endl;
}
// penetration points from sphereA into sphereB
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 AtoB = sphereB.getTranslation() - sphereA.getTranslation();
glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * glm::normalize(AtoB);
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
}
// collide B to A...
{
bool touching = ShapeCollider::collideShapes(&sphereB, &sphereA, collisions);
if (!touching) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphereA and sphereB should touch" << std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into sphereB
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
float inaccuracy = glm::length(collision->_penetration + expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 BtoA = sphereA.getTranslation() - sphereB.getTranslation();
glm::vec3 expectedContactPoint = sphereB.getTranslation() + radiusB * glm::normalize(BtoA);
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
}
}
void VerletShapeTests::sphereMissesCapsule() {
// non-overlapping sphere and capsule
float radiusA = 1.5f;
float radiusB = 2.3f;
float totalRadius = radiusA + radiusB;
float halfHeightB = 1.7f;
float axialOffset = totalRadius + 1.1f * halfHeightB;
float radialOffset = 1.2f * radiusA + 1.3f * radiusB;
// create points for the sphere + capsule
VerletPoint points[3];
for (int i = 0; i < 3; ++i) {
points[i]._position = glm::vec3(0.0f);
}
// give the points to the shapes
VerletSphereShape sphereA(radiusA, points);
VerletCapsuleShape capsuleB(radiusB, points+1, points+2);
capsuleB.setHalfHeight(halfHeightB);
// give the capsule some arbitrary transform
float angle = 37.8f;
glm::vec3 axis = glm::normalize( glm::vec3(-7.0f, 2.8f, 9.3f) );
glm::quat rotation = glm::angleAxis(angle, axis);
glm::vec3 translation(15.1f, -27.1f, -38.6f);
capsuleB.setRotation(rotation);
capsuleB.setTranslation(translation);
CollisionList collisions(16);
// walk sphereA along the local yAxis next to, but not touching, capsuleB
glm::vec3 localStartPosition(radialOffset, axialOffset, 0.0f);
int numberOfSteps = 10;
float delta = 1.3f * (totalRadius + halfHeightB) / (numberOfSteps - 1);
for (int i = 0; i < numberOfSteps; ++i) {
// translate sphereA into world-frame
glm::vec3 localPosition = localStartPosition + ((float)i * delta) * yAxis;
sphereA.setTranslation(rotation * localPosition + translation);
// sphereA agains capsuleB
if (ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should NOT touch" << std::endl;
}
// capsuleB against sphereA
if (ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should NOT touch" << std::endl;
}
}
if (collisions.size() > 0) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected empty collision list but size is " << collisions.size() << std::endl;
}
}
void VerletShapeTests::sphereTouchesCapsule() {
// overlapping sphere and capsule
float radiusA = 2.0f;
float radiusB = 1.0f;
float totalRadius = radiusA + radiusB;
float halfHeightB = 2.0f;
float alpha = 0.5f;
float beta = 0.5f;
float radialOffset = alpha * radiusA + beta * radiusB;
// create points for the sphere + capsule
VerletPoint points[3];
for (int i = 0; i < 3; ++i) {
points[i]._position = glm::vec3(0.0f);
}
// give the points to the shapes
VerletSphereShape sphereA(radiusA, points);
VerletCapsuleShape capsuleB(radiusB, points+1, points+2);
capsuleB.setHalfHeight(halfHeightB);
CollisionList collisions(16);
int numCollisions = 0;
{ // sphereA collides with capsuleB's cylindrical wall
sphereA.setTranslation(radialOffset * xAxis);
if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch" << std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = (radialOffset - totalRadius) * xAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * xAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch" << std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - (radialOffset - totalRadius) * xAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedPenetration *= -1.0f;
}
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 BtoA = sphereA.getTranslation() - capsuleB.getTranslation();
glm::vec3 closestApproach = capsuleB.getTranslation() + glm::dot(BtoA, yAxis) * yAxis;
expectedContactPoint = closestApproach + radiusB * glm::normalize(BtoA - closestApproach);
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
closestApproach = sphereA.getTranslation() - glm::dot(BtoA, yAxis) * yAxis;
expectedContactPoint = closestApproach - radiusB * glm::normalize(BtoA - closestApproach);
}
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
}
{ // sphereA hits end cap at axis
glm::vec3 axialOffset = (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
sphereA.setTranslation(axialOffset);
if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch" << std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 expectedContactPoint = sphereA.getTranslation() - radiusA * yAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch" << std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedPenetration *= -1.0f;
}
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 endPoint;
capsuleB.getEndPoint(endPoint);
expectedContactPoint = endPoint + radiusB * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedContactPoint = axialOffset - radiusA * yAxis;
}
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
}
{ // sphereA hits start cap at axis
glm::vec3 axialOffset = - (halfHeightB + alpha * radiusA + beta * radiusB) * yAxis;
sphereA.setTranslation(axialOffset);
if (!ShapeCollider::collideShapes(&sphereA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: sphere and capsule should touch" << std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of sphereA
glm::vec3 expectedContactPoint = sphereA.getTranslation() + radiusA * yAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB collides with sphereA
if (!ShapeCollider::collideShapes(&capsuleB, &sphereA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and sphere should touch" << std::endl;
} else {
++numCollisions;
}
// penetration points from sphereA into capsuleB
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - ((1.0f - alpha) * radiusA + (1.0f - beta) * radiusB) * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedPenetration *= -1.0f;
}
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
// contactPoint is on surface of capsuleB
glm::vec3 startPoint;
capsuleB.getStartPoint(startPoint);
expectedContactPoint = startPoint - radiusB * yAxis;
if (collision->_shapeA == &sphereA) {
// the ShapeCollider swapped the order of the shapes
expectedContactPoint = axialOffset + radiusA * yAxis;
}
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
}
if (collisions.size() != numCollisions) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected " << numCollisions << " collisions but actual number is " << collisions.size()
<< std::endl;
}
}
void VerletShapeTests::capsuleMissesCapsule() {
// non-overlapping capsules
float radiusA = 2.0f;
float halfHeightA = 3.0f;
float radiusB = 3.0f;
float halfHeightB = 4.0f;
float totalRadius = radiusA + radiusB;
float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
// create points for the shapes
VerletPoint points[4];
for (int i = 0; i < 4; ++i) {
points[i]._position = glm::vec3(0.0f);
}
// give the points to the shapes
VerletCapsuleShape capsuleA(radiusA, points+0, points+1);
VerletCapsuleShape capsuleB(radiusB, points+2, points+3);
capsuleA.setHalfHeight(halfHeightA);
capsuleA.setHalfHeight(halfHeightB);
CollisionList collisions(16);
// side by side
capsuleB.setTranslation((1.01f * totalRadius) * xAxis);
if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
// end to end
capsuleB.setTranslation((1.01f * totalHalfLength) * xAxis);
if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
// rotate B and move it to the side
glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis);
capsuleB.setRotation(rotation);
capsuleB.setTranslation((1.01f * (totalRadius + capsuleB.getHalfHeight())) * xAxis);
if (ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
if (ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should NOT touch" << std::endl;
}
if (collisions.size() > 0) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: expected empty collision list but size is " << collisions.size() << std::endl;
}
}
void VerletShapeTests::capsuleTouchesCapsule() {
// overlapping capsules
float radiusA = 2.0f;
float halfHeightA = 3.0f;
float radiusB = 3.0f;
float halfHeightB = 4.0f;
float totalRadius = radiusA + radiusB;
float totalHalfLength = totalRadius + halfHeightA + halfHeightB;
// create points for the shapes
VerletPoint points[4];
for (int i = 0; i < 4; ++i) {
points[i]._position = glm::vec3(0.0f);
}
// give the points to the shapes
VerletCapsuleShape capsuleA(radiusA, points+0, points+1);
VerletCapsuleShape capsuleB(radiusB, points+2, points+3);
capsuleA.setHalfHeight(halfHeightA);
capsuleB.setHalfHeight(halfHeightB);
CollisionList collisions(16);
int numCollisions = 0;
{ // side by side
capsuleB.setTranslation((0.99f * totalRadius) * xAxis);
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
}
{ // end to end
capsuleB.setTranslation((0.99f * totalHalfLength) * yAxis);
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
}
{ // rotate B and move it to the side
glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis);
capsuleB.setRotation(rotation);
capsuleB.setTranslation((0.99f * (totalRadius + capsuleB.getHalfHeight())) * xAxis);
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
}
{ // again, but this time check collision details
float overlap = 0.1f;
glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis);
capsuleB.setRotation(rotation);
glm::vec3 positionB = ((totalRadius + capsuleB.getHalfHeight()) - overlap) * xAxis;
capsuleB.setTranslation(positionB);
// capsuleA vs capsuleB
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = overlap * xAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * xAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
// capsuleB vs capsuleA
if (!ShapeCollider::collideShapes(&capsuleB, &capsuleA, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
collision = collisions.getCollision(numCollisions - 1);
expectedPenetration = - overlap * xAxis;
inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
expectedContactPoint = capsuleB.getTranslation() - (radiusB + halfHeightB) * xAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
}
{ // collide cylinder wall against cylinder wall
float overlap = 0.137f;
float shift = 0.317f * halfHeightA;
glm::quat rotation = glm::angleAxis(PI_OVER_TWO, zAxis);
capsuleB.setRotation(rotation);
glm::vec3 positionB = (totalRadius - overlap) * zAxis + shift * yAxis;
capsuleB.setTranslation(positionB);
// capsuleA vs capsuleB
if (!ShapeCollider::collideShapes(&capsuleA, &capsuleB, collisions))
{
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: capsule and capsule should touch" << std::endl;
} else {
++numCollisions;
}
CollisionInfo* collision = collisions.getCollision(numCollisions - 1);
glm::vec3 expectedPenetration = overlap * zAxis;
float inaccuracy = glm::length(collision->_penetration - expectedPenetration);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad penetration: expected = " << expectedPenetration
<< " actual = " << collision->_penetration << std::endl;
}
glm::vec3 expectedContactPoint = capsuleA.getTranslation() + radiusA * zAxis + shift * yAxis;
inaccuracy = glm::length(collision->_contactPoint - expectedContactPoint);
if (fabs(inaccuracy) > EPSILON) {
std::cout << __FILE__ << ":" << __LINE__
<< " ERROR: bad contactPoint: expected = " << expectedContactPoint
<< " actual = " << collision->_contactPoint << std::endl;
}
}
}
void VerletShapeTests::runAllTests() {
ShapeCollider::initDispatchTable();
setSpherePosition();
sphereMissesSphere();
sphereTouchesSphere();
sphereMissesCapsule();
sphereTouchesCapsule();
capsuleMissesCapsule();
capsuleTouchesCapsule();
}

30
tests/physics/src/VerletShapeTests.h
View file

@ -1,30 +0,0 @@
//
// VerletShapeTests.h
// tests/physics/src
//
// Created by Andrew Meadows on 2014.06.18
// 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_VerletShapeTests_h
#define hifi_VerletShapeTests_h
namespace VerletShapeTests {
void setSpherePosition();
void sphereMissesSphere();
void sphereTouchesSphere();
void sphereMissesCapsule();
void sphereTouchesCapsule();
void capsuleMissesCapsule();
void capsuleTouchesCapsule();
void runAllTests();
}
#endif // hifi_VerletShapeTests_h

2
tests/physics/src/main.cpp
View file

@ -9,7 +9,6 @@
//
#include "ShapeColliderTests.h"
#include "VerletShapeTests.h"
#include "ShapeInfoTests.h"
#include "ShapeManagerTests.h"
#include "BulletUtilTests.h"
@ -17,7 +16,6 @@
int main(int argc, char** argv) {
ShapeColliderTests::runAllTests();
VerletShapeTests::runAllTests();
ShapeInfoTests::runAllTests();
ShapeManagerTests::runAllTests();
BulletUtilTests::runAllTests();