diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index a8dfe1feb6..f59064732c 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -85,13 +85,13 @@ MyAvatar::MyAvatar() : } _skeletonModel.setEnableShapes(true); // The skeleton is both a PhysicsEntity and Ragdoll, so we add it to the simulation once for each type. - _physicsSimulation.addEntity(&_skeletonModel); - _physicsSimulation.addRagdoll(&_skeletonModel); + _physicsSimulation.setEntity(&_skeletonModel); + _physicsSimulation.setRagdoll(&_skeletonModel); } MyAvatar::~MyAvatar() { - _physicsSimulation.removeEntity(&_skeletonModel); - _physicsSimulation.removeRagdoll(&_skeletonModel); + _physicsSimulation.setRagdoll(NULL); + _physicsSimulation.setEntity(NULL); _lookAtTargetAvatar.clear(); } @@ -206,9 +206,10 @@ void MyAvatar::simulate(float deltaTime) { { PerformanceTimer perfTimer("ragdoll"); if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) { - const int minError = 0.01f; + const float minError = 0.01f; const float maxIterations = 10; const quint64 maxUsec = 2000; + _physicsSimulation.setTranslation(_position); _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec); } else { _skeletonModel.moveShapesTowardJoints(1.0f); @@ -235,12 +236,10 @@ void MyAvatar::simulate(float deltaTime) { } else { _trapDuration = 0.0f; } - /* TODO: Andrew to make this work if (_collisionGroups & COLLISION_GROUP_AVATARS) { PerformanceTimer perfTimer("avatars"); updateCollisionWithAvatars(deltaTime); } - */ } // consider updating our billboard @@ -1551,8 +1550,6 @@ bool findAvatarAvatarPenetration(const glm::vec3 positionA, float radiusA, float return false; } -const float BODY_COLLISION_RESOLUTION_TIMESCALE = 0.5f; // seconds - void MyAvatar::updateCollisionWithAvatars(float deltaTime) { // Reset detector for nearest avatar _distanceToNearestAvatar = std::numeric_limits::max(); @@ -1563,41 +1560,51 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) { } float myBoundingRadius = getBoundingRadius(); - const float BODY_COLLISION_RESOLUTION_FACTOR = glm::max(1.0f, deltaTime / BODY_COLLISION_RESOLUTION_TIMESCALE); - + // find nearest avatar + float nearestDistance2 = std::numeric_limits::max(); + Avatar* nearestAvatar = NULL; foreach (const AvatarSharedPointer& avatarPointer, avatars) { Avatar* avatar = static_cast(avatarPointer.data()); if (static_cast(this) == avatar) { // don't collide with ourselves continue; } - float distance = glm::length(_position - avatar->getPosition()); - if (_distanceToNearestAvatar > distance) { - _distanceToNearestAvatar = distance; + float distance2 = glm::distance2(_position, avatar->getPosition()); + if (nearestDistance2 > distance2) { + nearestDistance2 = distance2; + nearestAvatar = avatar; } - float theirBoundingRadius = avatar->getBoundingRadius(); - if (distance < myBoundingRadius + theirBoundingRadius) { - // collide our body against theirs - QVector myShapes; - _skeletonModel.getBodyShapes(myShapes); - QVector theirShapes; - avatar->getSkeletonModel().getBodyShapes(theirShapes); + } + _distanceToNearestAvatar = glm::sqrt(nearestDistance2); - CollisionInfo collision; - if (ShapeCollider::collideShapesCoarse(myShapes, theirShapes, collision)) { - float penetrationDepth = glm::length(collision._penetration); - if (penetrationDepth > myBoundingRadius) { - qDebug() << "WARNING: ignoring avatar-avatar penetration depth " << penetrationDepth; - } - else if (penetrationDepth > EPSILON) { - setPosition(getPosition() - BODY_COLLISION_RESOLUTION_FACTOR * collision._penetration); - _lastBodyPenetration += collision._penetration; - emit collisionWithAvatar(getSessionUUID(), avatar->getSessionUUID(), collision); - } + 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(avatarPointer.data()); + if (static_cast(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); + } + } else if (simulation == &(_physicsSimulation)) { + _physicsSimulation.removeRagdoll(skeleton); + _physicsSimulation.removeEntity(skeleton); + skeleton->setEnableShapes(false); } - - // collide their hands against us - avatar->getHand()->collideAgainstAvatar(this, false); } } } diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index c8e7451fc1..5e593526be 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -490,11 +490,13 @@ void SkeletonModel::renderRagdoll() { float alpha = 0.3f; float radius1 = 0.008f; float radius2 = 0.01f; + glm::vec3 simulationTranslation = getTranslationInSimulationFrame(); for (int i = 0; i < numPoints; ++i) { glPushMatrix(); - // draw each point as a yellow hexagon with black border - glm::vec3 position = _rotation * _ragdollPoints[i]._position; + // NOTE: ragdollPoints are in simulation-frame but we want them to be model-relative + glm::vec3 position = _ragdollPoints[i]._position - simulationTranslation; glTranslatef(position.x, position.y, position.z); + // draw each point as a yellow hexagon with black border glColor4f(0.0f, 0.0f, 0.0f, alpha); glutSolidSphere(radius2, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); glColor4f(1.0f, 1.0f, 0.0f, alpha); @@ -511,13 +513,14 @@ void SkeletonModel::initRagdollPoints() { clearRagdollConstraintsAndPoints(); _muscleConstraints.clear(); + initRagdollTransform(); // one point for each joint - int numJoints = _jointStates.size(); - _ragdollPoints.fill(VerletPoint(), numJoints); - for (int i = 0; i < numJoints; ++i) { + int numStates = _jointStates.size(); + _ragdollPoints.fill(VerletPoint(), numStates); + for (int i = 0; i < numStates; ++i) { const JointState& state = _jointStates.at(i); - glm::vec3 position = state.getPosition(); - _ragdollPoints[i].initPosition(position); + // _ragdollPoints start in model-frame + _ragdollPoints[i].initPosition(state.getPosition()); } } @@ -534,12 +537,12 @@ void SkeletonModel::buildRagdollConstraints() { const JointState& state = _jointStates.at(i); int parentIndex = state.getParentIndex(); if (parentIndex == -1) { - FixedConstraint* anchor = new FixedConstraint(&(_ragdollPoints[i]), glm::vec3(0.0f)); - _ragdollConstraints.push_back(anchor); + FixedConstraint* anchor = new FixedConstraint(&_translationInSimulationFrame, &(_ragdollPoints[i])); + _fixedConstraints.push_back(anchor); } else { DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[i]), &(_ragdollPoints[parentIndex])); bone->setDistance(state.getDistanceToParent()); - _ragdollConstraints.push_back(bone); + _boneConstraints.push_back(bone); families.insert(parentIndex, i); } float boneLength = glm::length(state.getPositionInParentFrame()); @@ -558,11 +561,11 @@ void SkeletonModel::buildRagdollConstraints() { if (numChildren > 1) { for (int i = 1; i < numChildren; ++i) { DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[children[i-1]]), &(_ragdollPoints[children[i]])); - _ragdollConstraints.push_back(bone); + _boneConstraints.push_back(bone); } if (numChildren > 2) { DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[children[numChildren-1]]), &(_ragdollPoints[children[0]])); - _ragdollConstraints.push_back(bone); + _boneConstraints.push_back(bone); } } ++itr; @@ -604,6 +607,7 @@ void SkeletonModel::updateVisibleJointStates() { } QVector 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); @@ -628,8 +632,9 @@ void SkeletonModel::updateVisibleJointStates() { // 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), - points[i] - points[parentIndex]); + invRotation * (points[i] - points[parentIndex])); // apply parentState.mixVisibleRotationDelta(delta, 0.01f); @@ -641,6 +646,7 @@ void SkeletonModel::updateVisibleJointStates() { // virtual void SkeletonModel::stepRagdollForward(float deltaTime) { + setRagdollTransform(_translation, _rotation); Ragdoll::stepRagdollForward(deltaTime); updateMuscles(); int numConstraints = _muscleConstraints.size(); @@ -665,6 +671,7 @@ void SkeletonModel::buildShapes() { } initRagdollPoints(); + float massScale = getMassScale(); float uniformScale = extractUniformScale(_scale); const int numStates = _jointStates.size(); @@ -678,25 +685,30 @@ void SkeletonModel::buildShapes() { // this shape is forced to be a sphere type = Shape::SPHERE_SHAPE; } + if (radius < EPSILON) { + type = Shape::UNKNOWN_SHAPE; + } Shape* shape = NULL; int parentIndex = joint.parentIndex; if (type == Shape::SPHERE_SHAPE) { shape = new VerletSphereShape(radius, &(_ragdollPoints[i])); shape->setEntity(this); - _ragdollPoints[i]._mass = glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()); + _ragdollPoints[i].setMass(massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume())); } else if (type == Shape::CAPSULE_SHAPE) { assert(parentIndex != -1); shape = new VerletCapsuleShape(radius, &(_ragdollPoints[parentIndex]), &(_ragdollPoints[i])); shape->setEntity(this); - _ragdollPoints[i]._mass = glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()); + _ragdollPoints[i].setMass(massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume())); } if (parentIndex != -1) { // always disable collisions between joint and its parent - disableCollisions(i, parentIndex); + if (shape) { + disableCollisions(i, parentIndex); + } } else { // give the base joint a very large mass since it doesn't actually move // in the local-frame simulation (it defines the origin) - _ragdollPoints[i]._mass = VERY_BIG_MASS; + _ragdollPoints[i].setMass(VERY_BIG_MASS); } _shapes.push_back(shape); } @@ -712,8 +724,9 @@ void SkeletonModel::buildShapes() { // ... then move shapes back to current joint positions if (_ragdollPoints.size() == numStates) { - int numJoints = _jointStates.size(); - for (int i = 0; i < numJoints; ++i) { + int numStates = _jointStates.size(); + for (int i = 0; i < numStates; ++i) { + // ragdollPoints start in model-frame _ragdollPoints[i].initPosition(_jointStates.at(i).getPosition()); } } @@ -735,9 +748,11 @@ void SkeletonModel::moveShapesTowardJoints(float deltaTime) { float fraction = glm::clamp(deltaTime / RAGDOLL_FOLLOWS_JOINTS_TIMESCALE, 0.0f, 1.0f); float oneMinusFraction = 1.0f - fraction; + glm::vec3 simulationTranslation = 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 * _jointStates.at(i).getPosition()); + fraction * (simulationTranslation + _rotation * (_jointStates.at(i).getPosition()))); } } @@ -748,23 +763,22 @@ void SkeletonModel::updateMuscles() { int j = constraint->getParentIndex(); int k = constraint->getChildIndex(); assert(j != -1 && k != -1); - constraint->setChildOffset(_jointStates.at(k).getPosition() - _jointStates.at(j).getPosition()); + // ragdollPoints are in simulation-frame but jointStates are in model-frame + constraint->setChildOffset(_rotation * (_jointStates.at(k).getPosition() - _jointStates.at(j).getPosition())); } } void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) { // compute default joint transforms - int numJoints = geometry.joints.size(); - if (numJoints != _ragdollPoints.size()) { - return; - } + int numStates = _jointStates.size(); QVector transforms; - transforms.fill(glm::mat4(), numJoints); + transforms.fill(glm::mat4(), numStates); // compute the default transforms and slam the ragdoll positions accordingly // (which puts the shapes where we want them) - for (int i = 0; i < numJoints; i++) { - const FBXJoint& joint = geometry.joints.at(i); + 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(); @@ -836,7 +850,7 @@ void SkeletonModel::resetShapePositionsToDefaultPose() { } const FBXGeometry& geometry = _geometry->getFBXGeometry(); - if (geometry.joints.isEmpty() || _shapes.size() != geometry.joints.size()) { + if (geometry.joints.isEmpty()) { return; } @@ -891,3 +905,54 @@ void SkeletonModel::renderBoundingCollisionShapes(float alpha) { glPopMatrix(); } +const int BALL_SUBDIVISIONS = 10; + +// virtual +void SkeletonModel::renderJointCollisionShapes(float alpha) { + glPushMatrix(); + Application::getInstance()->loadTranslatedViewMatrix(_translation); + glm::vec3 simulationTranslation = getTranslationInSimulationFrame(); + for (int i = 0; i < _shapes.size(); i++) { + Shape* shape = _shapes[i]; + if (!shape) { + continue; + } + + glPushMatrix(); + // shapes are stored in simulation-frame but we want position to be model-relative + if (shape->getType() == Shape::SPHERE_SHAPE) { + glm::vec3 position = shape->getTranslation() - simulationTranslation; + glTranslatef(position.x, position.y, position.z); + // draw a grey sphere at shape position + glColor4f(0.75f, 0.75f, 0.75f, alpha); + glutSolidSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); + } else if (shape->getType() == Shape::CAPSULE_SHAPE) { + CapsuleShape* capsule = static_cast(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); + glColor4f(0.6f, 0.6f, 0.8f, alpha); + glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); + + // 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); + glColor4f(0.8f, 0.8f, 0.6f, alpha); + glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); + + // draw a green cylinder between the two points + glm::vec3 origin(0.0f); + glColor4f(0.6f, 0.8f, 0.6f, alpha); + Avatar::renderJointConnectingCone( origin, axis, capsule->getRadius(), capsule->getRadius()); + } + glPopMatrix(); + } + glPopMatrix(); +} + diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h index 5cb89f5e44..55fedbcb6b 100644 --- a/interface/src/avatar/SkeletonModel.h +++ b/interface/src/avatar/SkeletonModel.h @@ -105,6 +105,7 @@ public: 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; } diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 63a94772a7..a915406f8e 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -188,10 +188,12 @@ void Model::initSkinProgram(ProgramObject& program, Model::SkinLocations& locati QVector Model::createJointStates(const FBXGeometry& geometry) { QVector jointStates; - foreach (const FBXJoint& joint, geometry.joints) { - // NOTE: the state keeps a pointer to an FBXJoint + for (int i = 0; i < geometry.joints.size(); ++i) { + const FBXJoint& joint = geometry.joints[i]; + // store a pointer to the FBXJoint in the JointState JointState state; state.setFBXJoint(&joint); + jointStates.append(state); } return jointStates; @@ -199,8 +201,8 @@ QVector Model::createJointStates(const FBXGeometry& geometry) { void Model::initJointTransforms() { // compute model transforms - int numJoints = _jointStates.size(); - for (int i = 0; i < numJoints; ++i) { + int numStates = _jointStates.size(); + for (int i = 0; i < numStates; ++i) { JointState& state = _jointStates[i]; const FBXJoint& joint = state.getFBXJoint(); int parentIndex = joint.parentIndex; @@ -538,9 +540,9 @@ void Model::setJointStates(QVector states) { _jointStates = states; initJointTransforms(); - int numJoints = _jointStates.size(); + int numStates = _jointStates.size(); float radius = 0.0f; - for (int i = 0; i < numJoints; ++i) { + for (int i = 0; i < numStates; ++i) { float distance = glm::length(_jointStates[i].getPosition()); if (distance > radius) { radius = distance; @@ -1243,55 +1245,7 @@ float Model::getLimbLength(int jointIndex) const { const int BALL_SUBDIVISIONS = 10; void Model::renderJointCollisionShapes(float alpha) { - glPushMatrix(); - Application::getInstance()->loadTranslatedViewMatrix(_translation); - for (int i = 0; i < _shapes.size(); i++) { - Shape* shape = _shapes[i]; - if (!shape) { - continue; - } - - glPushMatrix(); - // NOTE: the shapes are in the avatar local-frame - if (shape->getType() == Shape::SPHERE_SHAPE) { - // shapes are stored in world-frame, so we have to transform into model frame - glm::vec3 position = _rotation * shape->getTranslation(); - glTranslatef(position.x, position.y, position.z); - const glm::quat& rotation = shape->getRotation(); - glm::vec3 axis = glm::axis(rotation); - glRotatef(glm::degrees(glm::angle(rotation)), axis.x, axis.y, axis.z); - - // draw a grey sphere at shape position - glColor4f(0.75f, 0.75f, 0.75f, alpha); - glutSolidSphere(shape->getBoundingRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); - } else if (shape->getType() == Shape::CAPSULE_SHAPE) { - CapsuleShape* capsule = static_cast(shape); - - // draw a blue sphere at the capsule endpoint - glm::vec3 endPoint; - capsule->getEndPoint(endPoint); - endPoint = _rotation * endPoint; - glTranslatef(endPoint.x, endPoint.y, endPoint.z); - glColor4f(0.6f, 0.6f, 0.8f, alpha); - glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); - - // draw a yellow sphere at the capsule startpoint - glm::vec3 startPoint; - capsule->getStartPoint(startPoint); - startPoint = _rotation * startPoint; - glm::vec3 axis = endPoint - startPoint; - glTranslatef(-axis.x, -axis.y, -axis.z); - glColor4f(0.8f, 0.8f, 0.6f, alpha); - glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS); - - // draw a green cylinder between the two points - glm::vec3 origin(0.0f); - glColor4f(0.6f, 0.8f, 0.6f, alpha); - Avatar::renderJointConnectingCone( origin, axis, capsule->getRadius(), capsule->getRadius()); - } - glPopMatrix(); - } - glPopMatrix(); + // implement this when we have shapes for regular models } void Model::setBlendedVertices(const QVector& vertices, const QVector& normals) { diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index cbed941791..7a29b61420 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -149,7 +149,7 @@ public: virtual void buildShapes(); virtual void updateShapePositions(); - void renderJointCollisionShapes(float alpha); + virtual void renderJointCollisionShapes(float alpha); /// Sets blended vertices computed in a separate thread. void setBlendedVertices(const QVector& vertices, const QVector& normals); diff --git a/libraries/shared/src/FixedConstraint.cpp b/libraries/shared/src/FixedConstraint.cpp index 099c6d7bac..8f1edc1fb5 100644 --- a/libraries/shared/src/FixedConstraint.cpp +++ b/libraries/shared/src/FixedConstraint.cpp @@ -10,26 +10,26 @@ // #include "FixedConstraint.h" -#include "Shape.h" // for MAX_SHAPE_MASS #include "VerletPoint.h" -FixedConstraint::FixedConstraint(VerletPoint* point, const glm::vec3& anchor) : _point(point), _anchor(anchor) { +FixedConstraint::FixedConstraint(glm::vec3* anchor, VerletPoint* point) : _anchor(anchor), _point(point) { + assert(anchor); + assert(point); } float FixedConstraint::enforce() { assert(_point != NULL); - // TODO: use fast approximate sqrt here - float distance = glm::distance(_anchor, _point->_position); - _point->_position = _anchor; - return distance; + _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; - _point->_mass = MAX_SHAPE_MASS; -} - -void FixedConstraint::setAnchor(const glm::vec3& anchor) { - _anchor = anchor; } diff --git a/libraries/shared/src/FixedConstraint.h b/libraries/shared/src/FixedConstraint.h index 050232a027..66a27369ce 100644 --- a/libraries/shared/src/FixedConstraint.h +++ b/libraries/shared/src/FixedConstraint.h @@ -18,15 +18,19 @@ 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(VerletPoint* point, const glm::vec3& anchor); + FixedConstraint(glm::vec3* anchor, VerletPoint* point); + ~FixedConstraint() {} float enforce(); + void setAnchor(glm::vec3* anchor); void setPoint(VerletPoint* point); - void setAnchor(const glm::vec3& anchor); private: + glm::vec3* _anchor; VerletPoint* _point; - glm::vec3 _anchor; }; #endif // hifi_FixedConstraint_h diff --git a/libraries/shared/src/PhysicsSimulation.cpp b/libraries/shared/src/PhysicsSimulation.cpp index c2a852c32b..6dcafc6a54 100644 --- a/libraries/shared/src/PhysicsSimulation.cpp +++ b/libraries/shared/src/PhysicsSimulation.cpp @@ -24,19 +24,51 @@ int MAX_ENTITIES_PER_SIMULATION = 64; int MAX_COLLISIONS_PER_SIMULATION = 256; -PhysicsSimulation::PhysicsSimulation() : _frame(0), _collisions(MAX_COLLISIONS_PER_SIMULATION) { +PhysicsSimulation::PhysicsSimulation() : _translation(0.0f), _frameCount(0), _entity(NULL), _ragdoll(NULL), + _collisions(MAX_COLLISIONS_PER_SIMULATION) { } PhysicsSimulation::~PhysicsSimulation() { // entities have a backpointer to this simulator that must be cleaned up - int numEntities = _entities.size(); + int numEntities = _otherEntities.size(); for (int i = 0; i < numEntities; ++i) { - _entities[i]->_simulation = NULL; + _otherEntities[i]->_simulation = NULL; + } + _otherEntities.clear(); + if (_entity) { + _entity->_simulation = NULL; } - _entities.clear(); // but Ragdolls do not - _dolls.clear(); + _ragdoll = NULL; + _otherRagdolls.clear(); +} + +void PhysicsSimulation::setRagdoll(Ragdoll* ragdoll) { + if (_ragdoll != ragdoll) { + if (_ragdoll) { + _ragdoll->_ragdollSimulation = NULL; + } + _ragdoll = ragdoll; + if (_ragdoll) { + assert(!(_ragdoll->_ragdollSimulation)); + _ragdoll->_ragdollSimulation = 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) { @@ -44,25 +76,25 @@ bool PhysicsSimulation::addEntity(PhysicsEntity* entity) { return false; } if (entity->_simulation == this) { - int numEntities = _entities.size(); + int numEntities = _otherEntities.size(); for (int i = 0; i < numEntities; ++i) { - if (entity == _entities.at(i)) { + if (entity == _otherEntities.at(i)) { // already in list - assert(entity->_simulation == this); return true; } } // belongs to some other simulation return false; } - int numEntities = _entities.size(); + int numEntities = _otherEntities.size(); if (numEntities > MAX_ENTITIES_PER_SIMULATION) { // list is full return false; } // add to list + assert(!(entity->_simulation)); entity->_simulation = this; - _entities.push_back(entity); + _otherEntities.push_back(entity); return true; } @@ -71,17 +103,17 @@ void PhysicsSimulation::removeEntity(PhysicsEntity* entity) { return; } removeShapes(entity); - int numEntities = _entities.size(); + int numEntities = _otherEntities.size(); for (int i = 0; i < numEntities; ++i) { - if (entity == _entities.at(i)) { + if (entity == _otherEntities.at(i)) { if (i == numEntities - 1) { // remove it - _entities.pop_back(); + _otherEntities.pop_back(); } else { // swap the last for this one - PhysicsEntity* lastEntity = _entities[numEntities - 1]; - _entities.pop_back(); - _entities[i] = lastEntity; + PhysicsEntity* lastEntity = _otherEntities[numEntities - 1]; + _otherEntities.pop_back(); + _otherEntities[i] = lastEntity; } entity->_simulation = NULL; break; @@ -101,57 +133,75 @@ void PhysicsSimulation::removeShapes(const PhysicsEntity* entity) { } } +const float OTHER_RAGDOLL_MASS_SCALE = 10.0f; + bool PhysicsSimulation::addRagdoll(Ragdoll* doll) { if (!doll) { return false; } - int numDolls = _dolls.size(); + int numDolls = _otherRagdolls.size(); if (numDolls > MAX_DOLLS_PER_SIMULATION) { // list is full return false; } - for (int i = 0; i < numDolls; ++i) { - if (doll == _dolls[i]) { - // already in list - return true; + if (doll->_ragdollSimulation == this) { + for (int i = 0; i < numDolls; ++i) { + if (doll == _otherRagdolls[i]) { + // already in list + return true; + } } } // add to list - _dolls.push_back(doll); + assert(!(doll->_ragdollSimulation)); + doll->_ragdollSimulation = 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) { - int numDolls = _dolls.size(); + int numDolls = _otherRagdolls.size(); + if (doll->_ragdollSimulation != this) { + return; + } for (int i = 0; i < numDolls; ++i) { - if (doll == _dolls[i]) { + if (doll == _otherRagdolls[i]) { if (i == numDolls - 1) { // remove it - _dolls.pop_back(); + _otherRagdolls.pop_back(); } else { // swap the last for this one - Ragdoll* lastDoll = _dolls[numDolls - 1]; - _dolls.pop_back(); - _dolls[i] = lastDoll; + Ragdoll* lastDoll = _otherRagdolls[numDolls - 1]; + _otherRagdolls.pop_back(); + _otherRagdolls[i] = lastDoll; } + doll->_ragdollSimulation = NULL; + doll->setMassScale(1.0f); break; } } } void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) { - ++_frame; + ++_frameCount; + if (!_ragdoll) { + return; + } quint64 now = usecTimestampNow(); quint64 startTime = now; quint64 expiry = startTime + maxUsec; moveRagdolls(deltaTime); buildContactConstraints(); - int numDolls = _dolls.size(); + int numDolls = _otherRagdolls.size(); { PerformanceTimer perfTimer("enforce"); + _ragdoll->enforceRagdollConstraints(); for (int i = 0; i < numDolls; ++i) { - _dolls[i]->enforceRagdollConstraints(); + _otherRagdolls[i]->enforceRagdollConstraints(); } } @@ -164,9 +214,9 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter { // enforce constraints PerformanceTimer perfTimer("enforce"); - error = 0.0f; + error = _ragdoll->enforceRagdollConstraints(); for (int i = 0; i < numDolls; ++i) { - error = glm::max(error, _dolls[i]->enforceRagdollConstraints()); + error = glm::max(error, _otherRagdolls[i]->enforceRagdollConstraints()); } } enforceContactConstraints(); @@ -180,40 +230,38 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter void PhysicsSimulation::moveRagdolls(float deltaTime) { PerformanceTimer perfTimer("integrate"); - int numDolls = _dolls.size(); + _ragdoll->stepRagdollForward(deltaTime); + int numDolls = _otherRagdolls.size(); for (int i = 0; i < numDolls; ++i) { - _dolls.at(i)->stepRagdollForward(deltaTime); + _otherRagdolls[i]->stepRagdollForward(deltaTime); } } void PhysicsSimulation::computeCollisions() { PerformanceTimer perfTimer("collide"); _collisions.clear(); - // TODO: keep track of QSet collidedEntities; - int numEntities = _entities.size(); - for (int i = 0; i < numEntities; ++i) { - PhysicsEntity* entity = _entities.at(i); - const QVector shapes = entity->getShapes(); - int numShapes = shapes.size(); - // collide with self - for (int j = 0; j < numShapes; ++j) { - const Shape* shape = shapes.at(j); - if (!shape) { - continue; - } - for (int k = j+1; k < numShapes; ++k) { - const Shape* otherShape = shapes.at(k); - if (otherShape && entity->collisionsAreEnabled(j, k)) { - ShapeCollider::collideShapes(shape, otherShape, _collisions); - } - } - } - // collide with others - for (int j = i+1; j < numEntities; ++j) { - const QVector otherShapes = _entities.at(j)->getShapes(); - ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions); + const QVector 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 + int numEntities = _otherEntities.size(); + for (int i = 0; i < numEntities; ++i) { + const QVector otherShapes = _otherEntities.at(i)->getShapes(); + ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions); } } @@ -269,9 +317,9 @@ void PhysicsSimulation::updateContacts() { } QMap::iterator itr = _contacts.find(key); if (itr == _contacts.end()) { - _contacts.insert(key, ContactPoint(*collision, _frame)); + _contacts.insert(key, ContactPoint(*collision, _frameCount)); } else { - itr.value().updateContact(*collision, _frame); + itr.value().updateContact(*collision, _frameCount); } } } @@ -281,7 +329,7 @@ const quint32 MAX_CONTACT_FRAME_LIFETIME = 2; void PhysicsSimulation::pruneContacts() { QMap::iterator itr = _contacts.begin(); while (itr != _contacts.end()) { - if (_frame - itr.value().getLastFrame() > MAX_CONTACT_FRAME_LIFETIME) { + if (_frameCount - itr.value().getLastFrame() > MAX_CONTACT_FRAME_LIFETIME) { itr = _contacts.erase(itr); } else { ++itr; diff --git a/libraries/shared/src/PhysicsSimulation.h b/libraries/shared/src/PhysicsSimulation.h index 506b37533a..61ab1bf177 100644 --- a/libraries/shared/src/PhysicsSimulation.h +++ b/libraries/shared/src/PhysicsSimulation.h @@ -28,6 +28,12 @@ public: PhysicsSimulation(); ~PhysicsSimulation(); + 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); @@ -56,10 +62,15 @@ protected: void pruneContacts(); private: - quint32 _frame; + glm::vec3 _translation; // origin of simulation in world-frame - QVector _dolls; - QVector _entities; + quint32 _frameCount; + + PhysicsEntity* _entity; + Ragdoll* _ragdoll; + + QVector _otherRagdolls; + QVector _otherEntities; CollisionList _collisions; QMap _contacts; }; diff --git a/libraries/shared/src/Ragdoll.cpp b/libraries/shared/src/Ragdoll.cpp index 6282db4dfb..e10e1620ae 100644 --- a/libraries/shared/src/Ragdoll.cpp +++ b/libraries/shared/src/Ragdoll.cpp @@ -9,13 +9,17 @@ // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // +#include + #include "Ragdoll.h" #include "Constraint.h" #include "DistanceConstraint.h" #include "FixedConstraint.h" +#include "PhysicsSimulation.h" +#include "SharedUtil.h" // for EPSILON -Ragdoll::Ragdoll() { +Ragdoll::Ragdoll() : _massScale(1.0f), _ragdollTranslation(0.0f), _translationInSimulationFrame(0.0f), _ragdollSimulation(NULL) { } Ragdoll::~Ragdoll() { @@ -23,6 +27,9 @@ Ragdoll::~Ragdoll() { } void Ragdoll::stepRagdollForward(float deltaTime) { + if (_ragdollSimulation) { + updateSimulationTransforms(_ragdollTranslation - _ragdollSimulation->getTranslation(), _ragdollRotation); + } int numPoints = _ragdollPoints.size(); for (int i = 0; i < numPoints; ++i) { _ragdollPoints[i].integrateForward(); @@ -30,21 +37,79 @@ void Ragdoll::stepRagdollForward(float deltaTime) { } void Ragdoll::clearRagdollConstraintsAndPoints() { - int numConstraints = _ragdollConstraints.size(); + int numConstraints = _boneConstraints.size(); for (int i = 0; i < numConstraints; ++i) { - delete _ragdollConstraints[i]; + delete _boneConstraints[i]; } - _ragdollConstraints.clear(); + _boneConstraints.clear(); + numConstraints = _fixedConstraints.size(); + for (int i = 0; i < numConstraints; ++i) { + delete _fixedConstraints[i]; + } + _fixedConstraints.clear(); _ragdollPoints.clear(); } float Ragdoll::enforceRagdollConstraints() { float maxDistance = 0.0f; - const int numConstraints = _ragdollConstraints.size(); + // enforce the bone constraints first + int numConstraints = _boneConstraints.size(); for (int i = 0; i < numConstraints; ++i) { - DistanceConstraint* c = static_cast(_ragdollConstraints[i]); - //maxDistance = glm::max(maxDistance, _ragdollConstraints[i]->enforce()); - maxDistance = glm::max(maxDistance, c->enforce()); + 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::initRagdollTransform() { + _ragdollTranslation = glm::vec3(0.0f); + _ragdollRotation = glm::quat(); + _translationInSimulationFrame = glm::vec3(0.0f); + _rotationInSimulationFrame = glm::quat(); +} + +void Ragdoll::setRagdollTransform(const glm::vec3& translation, const glm::quat& rotation) { + _ragdollTranslation = translation; + _ragdollRotation = 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 = _ragdollPoints.size(); + for (int i = 0; i < numPoints; ++i) { + _ragdollPoints[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 = _ragdollPoints.size(); + for (int i = 0; i < numPoints; ++i) { + _ragdollPoints[i].setMass(rescale * _ragdollPoints[i].getMass()); + } + _massScale = scale; + } +} diff --git a/libraries/shared/src/Ragdoll.h b/libraries/shared/src/Ragdoll.h index 91a7e7330e..ec5a561d1a 100644 --- a/libraries/shared/src/Ragdoll.h +++ b/libraries/shared/src/Ragdoll.h @@ -18,7 +18,14 @@ #include -class Constraint; +//#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: @@ -35,13 +42,35 @@ public: const QVector& getRagdollPoints() const { return _ragdollPoints; } QVector& getRagdollPoints() { return _ragdollPoints; } + void initRagdollTransform(); + + /// set the translation and rotation of the Ragdoll and adjust all VerletPoints. + void setRagdollTransform(const glm::vec3& translation, const glm::quat& rotation); + + const glm::vec3& getTranslationInSimulationFrame() const { return _translationInSimulationFrame; } + + void setMassScale(float scale); + float getMassScale() const { return _massScale; } + protected: void clearRagdollConstraintsAndPoints(); virtual void initRagdollPoints() = 0; virtual void buildRagdollConstraints() = 0; + float _massScale; + glm::vec3 _ragdollTranslation; // world-frame + glm::quat _ragdollRotation; // world-frame + glm::vec3 _translationInSimulationFrame; + glm::quat _rotationInSimulationFrame; + QVector _ragdollPoints; - QVector _ragdollConstraints; + QVector _boneConstraints; + QVector _fixedConstraints; +private: + void updateSimulationTransforms(const glm::vec3& translation, const glm::quat& rotation); + + friend class PhysicsSimulation; + PhysicsSimulation* _ragdollSimulation; }; #endif // hifi_Ragdoll_h diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h index 015758427a..2a4d4143dc 100644 --- a/libraries/shared/src/SharedUtil.h +++ b/libraries/shared/src/SharedUtil.h @@ -122,4 +122,5 @@ bool isNaN(float value); QString formatUsecTime(float usecs, int prec = 3); + #endif // hifi_SharedUtil_h diff --git a/libraries/shared/src/VerletCapsuleShape.cpp b/libraries/shared/src/VerletCapsuleShape.cpp index 6f547d2048..ce324a781a 100644 --- a/libraries/shared/src/VerletCapsuleShape.cpp +++ b/libraries/shared/src/VerletCapsuleShape.cpp @@ -97,7 +97,7 @@ float VerletCapsuleShape::computeEffectiveMass(const glm::vec3& penetration, con _endLagrangeCoef = 1.0f; } // the effective mass is the weighted sum of the two endpoints - return _startLagrangeCoef * _startPoint->_mass + _endLagrangeCoef * _endPoint->_mass; + return _startLagrangeCoef * _startPoint->getMass() + _endLagrangeCoef * _endPoint->getMass(); } void VerletCapsuleShape::accumulateDelta(float relativeMassFactor, const glm::vec3& penetration) { diff --git a/libraries/shared/src/VerletPoint.cpp b/libraries/shared/src/VerletPoint.cpp index 654a74d7ac..d2dd985587 100644 --- a/libraries/shared/src/VerletPoint.cpp +++ b/libraries/shared/src/VerletPoint.cpp @@ -31,3 +31,19 @@ void VerletPoint::applyAccumulatedDelta() { _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::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); +} diff --git a/libraries/shared/src/VerletPoint.h b/libraries/shared/src/VerletPoint.h index f59afb16c5..6f94656966 100644 --- a/libraries/shared/src/VerletPoint.h +++ b/libraries/shared/src/VerletPoint.h @@ -13,6 +13,8 @@ #define hifi_VerletPoint_h #include +#include + class VerletPoint { public: @@ -22,12 +24,16 @@ public: 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 setMass(float mass); + float getMass() const { return _mass; } glm::vec3 _position; glm::vec3 _lastPosition; - float _mass; private: + float _mass; glm::vec3 _accumulatedDelta; int _numDeltas; }; diff --git a/libraries/shared/src/VerletSphereShape.cpp b/libraries/shared/src/VerletSphereShape.cpp index e24465fd89..da8242f26a 100644 --- a/libraries/shared/src/VerletSphereShape.cpp +++ b/libraries/shared/src/VerletSphereShape.cpp @@ -36,7 +36,7 @@ const glm::vec3& VerletSphereShape::getTranslation() const { // virtual float VerletSphereShape::computeEffectiveMass(const glm::vec3& penetration, const glm::vec3& contactPoint) { - return _point->_mass; + return _point->getMass(); } // virtual