void AvatarActionHold::updateActionWorker(float deltaTimeStep) {
bool gotLock = false;
glm::quat rotation;
glm::vec3 position;
std::shared_ptr<Avatar> holdingAvatar = nullptr;
gotLock = withTryReadLock([&] {
QSharedPointer<AvatarManager> avatarManager = DependencyManager::get<AvatarManager>();
AvatarSharedPointer holdingAvatarData = avatarManager->getAvatarBySessionID(_holderID);
holdingAvatar = std::static_pointer_cast<Avatar>(holdingAvatarData);
if (holdingAvatar) {
glm::vec3 offset;
glm::vec3 palmPosition;
glm::quat palmRotation;
if (_hand == "right") {
palmPosition = holdingAvatar->getRightPalmPosition();
palmRotation = holdingAvatar->getRightPalmRotation();
} else {
palmPosition = holdingAvatar->getLeftPalmPosition();
palmRotation = holdingAvatar->getLeftPalmRotation();
}
rotation = palmRotation * _relativeRotation;
offset = rotation * _relativePosition;
position = palmPosition + offset;
}
});
if (holdingAvatar) {
if (gotLock) {
gotLock = withTryWriteLock([&] {
_positionalTarget = position;
_rotationalTarget = rotation;
_positionalTargetSet = true;
_rotationalTargetSet = true;
_active = true;
});
if (gotLock) {
if (_kinematic) {
doKinematicUpdate(deltaTimeStep);
} else {
activateBody();
ObjectActionSpring::updateActionWorker(deltaTimeStep);
}
}
}
}
}
void AvatarActionHold::updateActionWorker(float deltaTimeStep) {
if (!_mine) {
// if a local script isn't updating this, then we are just getting spring-action data over the wire.
// let the super-class handle it.
ObjectActionSpring::updateActionWorker(deltaTimeStep);
return;
}
glm::quat rotation;
glm::vec3 position;
glm::vec3 offset;
bool gotLock = withTryReadLock([&]{
auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
glm::vec3 palmPosition;
glm::quat palmRotation;
if (_hand == "right") {
palmPosition = myAvatar->getRightPalmPosition();
palmRotation = myAvatar->getRightPalmRotation();
} else {
palmPosition = myAvatar->getLeftPalmPosition();
palmRotation = myAvatar->getLeftPalmRotation();
}
rotation = palmRotation * _relativeRotation;
offset = rotation * _relativePosition;
position = palmPosition + offset;
});
if (gotLock) {
gotLock = withTryWriteLock([&]{
if (_positionalTarget != position || _rotationalTarget != rotation) {
auto ownerEntity = _ownerEntity.lock();
if (ownerEntity) {
ownerEntity->setActionDataDirty(true);
}
_positionalTarget = position;
_rotationalTarget = rotation;
}
});
}
if (gotLock) {
ObjectActionSpring::updateActionWorker(deltaTimeStep);
}
}
void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) {
withTryReadLock([&] {
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
return;
}
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
btRigidBody* rigidBody = motionState->getRigidBody();
if (!rigidBody) {
qDebug() << "ObjectActionOffset::updateActionWorker no rigidBody";
return;
}
const float MAX_LINEAR_TIMESCALE = 600.0f; // 10 minutes is a long time
if (_positionalTargetSet && _linearTimeScale < MAX_LINEAR_TIMESCALE) {
glm::vec3 objectPosition = bulletToGLM(rigidBody->getCenterOfMassPosition());
glm::vec3 springAxis = objectPosition - _pointToOffsetFrom; // from anchor to object
float distance = glm::length(springAxis);
if (distance > FLT_EPSILON) {
springAxis /= distance; // normalize springAxis
// compute (critically damped) target velocity of spring relaxation
glm::vec3 offset = (distance - _linearDistance) * springAxis;
glm::vec3 targetVelocity = (-1.0f / _linearTimeScale) * offset;
// compute current velocity and its parallel component
glm::vec3 currentVelocity = bulletToGLM(rigidBody->getLinearVelocity());
glm::vec3 parallelVelocity = glm::dot(currentVelocity, springAxis) * springAxis;
// we blend the parallel component with the spring's target velocity to get the new velocity
float blend = deltaTimeStep / _linearTimeScale;
if (blend > 1.0f) {
blend = 1.0f;
}
rigidBody->setLinearVelocity(glmToBullet(currentVelocity + blend * (targetVelocity - parallelVelocity)));
}
}
});
}
void AvatarActionHold::updateActionWorker(float deltaTimeStep) {
bool gotLock = false;
glm::quat rotation;
glm::vec3 position;
glm::vec3 offset;
gotLock = withTryReadLock([&]{
auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
glm::vec3 palmPosition;
glm::quat palmRotation;
if (_hand == "right") {
palmPosition = myAvatar->getRightPalmPosition();
palmRotation = myAvatar->getRightPalmRotation();
} else {
palmPosition = myAvatar->getLeftPalmPosition();
palmRotation = myAvatar->getLeftPalmRotation();
}
rotation = palmRotation * _relativeRotation;
offset = rotation * _relativePosition;
position = palmPosition + offset;
});
if (gotLock) {
gotLock = withTryWriteLock([&]{
_positionalTarget = position;
_rotationalTarget = rotation;
_positionalTargetSet = true;
_rotationalTargetSet = true;
auto ownerEntity = _ownerEntity.lock();
if (ownerEntity) {
ownerEntity->setActionDataDirty(true);
}
});
}
if (gotLock) {
ObjectActionSpring::updateActionWorker(deltaTimeStep);
}
}
void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) {
// don't risk hanging the thread running the physics simulation
auto lockResult = withTryReadLock([&]{
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
return;
}
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
btRigidBody* rigidBody = motionState->getRigidBody();
if (!rigidBody) {
qDebug() << "ObjectActionSpring::updateActionWorker no rigidBody";
return;
}
const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
if (_linearTimeScale < MAX_TIMESCALE) {
btVector3 targetVelocity(0.0f, 0.0f, 0.0f);
btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget);
float offsetLength = offset.length();
if (offsetLength > FLT_EPSILON) {
float speed = glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED);
targetVelocity = (-speed / offsetLength) * offset;
if (speed > rigidBody->getLinearSleepingThreshold()) {
rigidBody->activate();
}
}
// this action is aggresively critically damped and defeats the current velocity
rigidBody->setLinearVelocity(targetVelocity);
}
if (_angularTimeScale < MAX_TIMESCALE) {
btVector3 targetVelocity(0.0f, 0.0f, 0.0f);
btQuaternion bodyRotation = rigidBody->getOrientation();
auto alignmentDot = bodyRotation.dot(glmToBullet(_rotationalTarget));
const float ALMOST_ONE = 0.99999f;
if (glm::abs(alignmentDot) < ALMOST_ONE) {
btQuaternion target = glmToBullet(_rotationalTarget);
if (alignmentDot < 0.0f) {
target = -target;
}
// if dQ is the incremental rotation that gets an object from Q0 to Q1 then:
//
// Q1 = dQ * Q0
//
// solving for dQ gives:
//
// dQ = Q1 * Q0^
btQuaternion deltaQ = target * bodyRotation.inverse();
float speed = deltaQ.getAngle() / _angularTimeScale;
targetVelocity = speed * deltaQ.getAxis();
if (speed > rigidBody->getAngularSleepingThreshold()) {
rigidBody->activate();
}
}
// this action is aggresively critically damped and defeats the current velocity
rigidBody->setAngularVelocity(targetVelocity);
}
});
if (!lockResult) {
qDebug() << "ObjectActionSpring::updateActionWorker lock failed";
}
}
