bool ObjectActionOffset::updateArguments(QVariantMap arguments) {
glm::vec3 pointToOffsetFrom;
float linearTimeScale;
float linearDistance;
bool needUpdate = false;
bool somethingChanged = ObjectAction::updateArguments(arguments);
withReadLock([&] {
bool ok = true;
pointToOffsetFrom =
EntityActionInterface::extractVec3Argument("offset action", arguments, "pointToOffsetFrom", ok, true);
if (!ok) {
pointToOffsetFrom = _pointToOffsetFrom;
}
ok = true;
linearTimeScale =
EntityActionInterface::extractFloatArgument("offset action", arguments, "linearTimeScale", ok, false);
if (!ok) {
linearTimeScale = _linearTimeScale;
}
ok = true;
linearDistance =
EntityActionInterface::extractFloatArgument("offset action", arguments, "linearDistance", ok, false);
if (!ok) {
linearDistance = _linearDistance;
}
// only change stuff if something actually changed
if (somethingChanged ||
_pointToOffsetFrom != pointToOffsetFrom ||
_linearTimeScale != linearTimeScale ||
_linearDistance != linearDistance) {
needUpdate = true;
}
});
if (needUpdate) {
withWriteLock([&] {
_pointToOffsetFrom = pointToOffsetFrom;
_linearTimeScale = linearTimeScale;
_linearDistance = linearDistance;
_positionalTargetSet = true;
_active = true;
auto ownerEntity = _ownerEntity.lock();
if (ownerEntity) {
ownerEntity->setActionDataDirty(true);
}
});
activateBody();
}
return true;
}
btTypedConstraint* ObjectConstraintBallSocket::getConstraint() {
btPoint2PointConstraint* constraint { nullptr };
QUuid otherEntityID;
glm::vec3 pivotInA;
glm::vec3 pivotInB;
withReadLock([&]{
constraint = static_cast<btPoint2PointConstraint*>(_constraint);
pivotInA = _pivotInA;
otherEntityID = _otherID;
pivotInB = _pivotInB;
});
if (constraint) {
return constraint;
}
static QString repeatedBallSocketNoRigidBody = LogHandler::getInstance().addRepeatedMessageRegex(
"ObjectConstraintBallSocket::getConstraint -- no rigidBody.*");
btRigidBody* rigidBodyA = getRigidBody();
if (!rigidBodyA) {
qCDebug(physics) << "ObjectConstraintBallSocket::getConstraint -- no rigidBodyA";
return nullptr;
}
if (!otherEntityID.isNull()) {
// This constraint is between two entities... find the other rigid body.
btRigidBody* rigidBodyB = getOtherRigidBody(otherEntityID);
if (!rigidBodyB) {
qCDebug(physics) << "ObjectConstraintBallSocket::getConstraint -- no rigidBodyB";
return nullptr;
}
constraint = new btPoint2PointConstraint(*rigidBodyA, *rigidBodyB, glmToBullet(pivotInA), glmToBullet(pivotInB));
} else {
// This constraint is between an entity and the world-frame.
constraint = new btPoint2PointConstraint(*rigidBodyA, glmToBullet(pivotInA));
}
withWriteLock([&]{
_constraint = constraint;
});
// if we don't wake up rigidBodyA, we may not send the dynamicData property over the network
forceBodyNonStatic();
activateBody();
updateBallSocket();
return constraint;
}
void AvatarActionHold::prepareForPhysicsSimulation() {
auto avatarManager = DependencyManager::get<AvatarManager>();
auto holdingAvatar = std::static_pointer_cast<Avatar>(avatarManager->getAvatarBySessionID(_holderID));
if (!holdingAvatar || !holdingAvatar->isMyAvatar()) {
return;
}
withWriteLock([&]{
glm::vec3 avatarRigidBodyPosition;
glm::quat avatarRigidBodyRotation;
getAvatarRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
if (_ignoreIK) {
return;
}
glm::vec3 palmPosition;
glm::quat palmRotation;
if (_hand == "right") {
palmPosition = holdingAvatar->getUncachedRightPalmPosition();
palmRotation = holdingAvatar->getUncachedRightPalmRotation();
} else {
palmPosition = holdingAvatar->getUncachedLeftPalmPosition();
palmRotation = holdingAvatar->getUncachedLeftPalmRotation();
}
// determine the difference in translation and rotation between the avatar's
// rigid body and the palm position. The avatar's rigid body will be moved by bullet
// between this call and the call to getTarget, below. A call to get*PalmPosition in
// getTarget would get the palm position of the previous location of the avatar (because
// bullet has moved the av's rigid body but the rigid body's location has not yet been
// copied out into the Avatar class.
//glm::quat avatarRotationInverse = glm::inverse(avatarRigidBodyRotation);
// the offset should be in the frame of the avatar, but something about the order
// things are updated makes this wrong:
// _palmOffsetFromRigidBody = avatarRotationInverse * (palmPosition - avatarRigidBodyPosition);
// I'll leave it here as a comment in case avatar handling changes.
_palmOffsetFromRigidBody = palmPosition - avatarRigidBodyPosition;
// rotation should also be needed, but again, the order of updates makes this unneeded. leaving
// code here for future reference.
// _palmRotationFromRigidBody = avatarRotationInverse * palmRotation;
});
activateBody(true);
}
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);
}
}
}
}
}
bool AvatarActionHold::updateArguments(QVariantMap arguments) {
bool ok = true;
glm::vec3 relativePosition =
EntityActionInterface::extractVec3Argument("hold", arguments, "relativePosition", ok, false);
if (!ok) {
relativePosition = _relativePosition;
}
ok = true;
glm::quat relativeRotation =
EntityActionInterface::extractQuatArgument("hold", arguments, "relativeRotation", ok, false);
if (!ok) {
relativeRotation = _relativeRotation;
}
ok = true;
float timeScale =
EntityActionInterface::extractFloatArgument("hold", arguments, "timeScale", ok, false);
if (!ok) {
timeScale = _linearTimeScale;
}
ok = true;
QString hand =
EntityActionInterface::extractStringArgument("hold", arguments, "hand", ok, false);
if (!ok || !(hand == "left" || hand == "right")) {
hand = _hand;
}
if (relativePosition != _relativePosition
|| relativeRotation != _relativeRotation
|| timeScale != _linearTimeScale
|| hand != _hand) {
withWriteLock([&] {
_relativePosition = relativePosition;
_relativeRotation = relativeRotation;
const float MIN_TIMESCALE = 0.1f;
_linearTimeScale = glm::min(MIN_TIMESCALE, timeScale);
_angularTimeScale = _linearTimeScale;
_hand = hand;
_mine = true;
_active = true;
activateBody();
});
}
return true;
}
bool ObjectActionSpring::updateArguments(QVariantMap arguments) {
// targets are required, spring-constants are optional
bool ok = true;
glm::vec3 positionalTarget =
EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ok, false);
if (!ok) {
positionalTarget = _positionalTarget;
}
ok = true;
float linearTimeScale =
EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", ok, false);
if (!ok || linearTimeScale <= 0.0f) {
linearTimeScale = _linearTimeScale;
}
ok = true;
glm::quat rotationalTarget =
EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", ok, false);
if (!ok) {
rotationalTarget = _rotationalTarget;
}
ok = true;
float angularTimeScale =
EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", ok, false);
if (!ok) {
angularTimeScale = _angularTimeScale;
}
if (positionalTarget != _positionalTarget
|| linearTimeScale != _linearTimeScale
|| rotationalTarget != _rotationalTarget
|| angularTimeScale != _angularTimeScale) {
// something changed
lockForWrite();
_positionalTarget = positionalTarget;
_linearTimeScale = glm::max(MIN_TIMESCALE, glm::abs(linearTimeScale));
_rotationalTarget = rotationalTarget;
_angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale));
_active = true;
activateBody();
unlock();
}
return true;
}
bool ObjectActionTravelOriented::updateArguments(QVariantMap arguments) {
glm::vec3 forward;
float angularTimeScale;
bool needUpdate = false;
bool somethingChanged = ObjectDynamic::updateArguments(arguments);
withReadLock([&]{
bool ok = true;
forward = EntityDynamicInterface::extractVec3Argument("travel oriented action", arguments, "forward", ok, true);
if (!ok) {
forward = _forward;
}
ok = true;
angularTimeScale =
EntityDynamicInterface::extractFloatArgument("travel oriented action", arguments, "angularTimeScale", ok, false);
if (!ok) {
angularTimeScale = _angularTimeScale;
}
if (somethingChanged ||
forward != _forward ||
angularTimeScale != _angularTimeScale) {
// something changed
needUpdate = true;
}
});
if (needUpdate) {
withWriteLock([&] {
_forward = forward;
_angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale));
_active = (_forward != glm::vec3());
auto ownerEntity = _ownerEntity.lock();
if (ownerEntity) {
ownerEntity->setDynamicDataDirty(true);
ownerEntity->setDynamicDataNeedsTransmit(true);
}
});
activateBody();
}
return true;
}
bool ObjectActionOffset::updateArguments(QVariantMap arguments) {
if (!ObjectAction::updateArguments(arguments)) {
return false;
}
bool ok = true;
glm::vec3 pointToOffsetFrom =
EntityActionInterface::extractVec3Argument("offset action", arguments, "pointToOffsetFrom", ok, true);
if (!ok) {
pointToOffsetFrom = _pointToOffsetFrom;
}
ok = true;
float linearTimeScale =
EntityActionInterface::extractFloatArgument("offset action", arguments, "linearTimeScale", ok, false);
if (!ok) {
linearTimeScale = _linearTimeScale;
}
ok = true;
float linearDistance =
EntityActionInterface::extractFloatArgument("offset action", arguments, "linearDistance", ok, false);
if (!ok) {
linearDistance = _linearDistance;
}
// only change stuff if something actually changed
if (_pointToOffsetFrom != pointToOffsetFrom
|| _linearTimeScale != linearTimeScale
|| _linearDistance != linearDistance) {
withWriteLock([&] {
_pointToOffsetFrom = pointToOffsetFrom;
_linearTimeScale = linearTimeScale;
_linearDistance = linearDistance;
_positionalTargetSet = true;
_active = true;
activateBody();
});
}
return true;
}
void AvatarActionHold::doKinematicUpdate(float deltaTimeStep) {
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning entity";
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning physics info";
return;
}
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
btRigidBody* rigidBody = motionState ? motionState->getRigidBody() : nullptr;
if (!rigidBody) {
qDebug() << "AvatarActionHold::doKinematicUpdate -- no rigidBody";
return;
}
withWriteLock([&] {
if (_kinematicSetVelocity) {
if (_previousSet) {
glm::vec3 positionalVelocity = (_positionalTarget - _previousPositionalTarget) / deltaTimeStep;
rigidBody->setLinearVelocity(glmToBullet(positionalVelocity));
}
}
btTransform worldTrans = rigidBody->getWorldTransform();
worldTrans.setOrigin(glmToBullet(_positionalTarget));
worldTrans.setRotation(glmToBullet(_rotationalTarget));
rigidBody->setWorldTransform(worldTrans);
motionState->dirtyInternalKinematicChanges();
_previousPositionalTarget = _positionalTarget;
_previousRotationalTarget = _rotationalTarget;
_previousSet = true;
});
activateBody();
}
btTypedConstraint* ObjectConstraintSlider::getConstraint() {
btSliderConstraint* constraint { nullptr };
QUuid otherEntityID;
glm::vec3 pointInA;
glm::vec3 axisInA;
glm::vec3 pointInB;
glm::vec3 axisInB;
withReadLock([&]{
constraint = static_cast<btSliderConstraint*>(_constraint);
pointInA = _pointInA;
axisInA = _axisInA;
otherEntityID = _otherID;
pointInB = _pointInB;
axisInB = _axisInB;
});
if (constraint) {
return constraint;
}
static int repeatMessageID = LogHandler::getInstance().newRepeatedMessageID();
btRigidBody* rigidBodyA = getRigidBody();
if (!rigidBodyA) {
HIFI_FCDEBUG_ID(physics(), repeatMessageID, "ObjectConstraintSlider::getConstraint -- no rigidBodyA");
return nullptr;
}
if (glm::length(axisInA) < FLT_EPSILON) {
qCWarning(physics) << "slider axis cannot be a zero vector";
axisInA = DEFAULT_SLIDER_AXIS;
} else {
axisInA = glm::normalize(axisInA);
}
if (!otherEntityID.isNull()) {
// This slider is between two entities... find the other rigid body.
if (glm::length(axisInB) < FLT_EPSILON) {
qCWarning(physics) << "slider axis cannot be a zero vector";
axisInB = DEFAULT_SLIDER_AXIS;
} else {
axisInB = glm::normalize(axisInB);
}
glm::quat rotA = glm::rotation(DEFAULT_SLIDER_AXIS, axisInA);
glm::quat rotB = glm::rotation(DEFAULT_SLIDER_AXIS, axisInB);
btTransform frameInA(glmToBullet(rotA), glmToBullet(pointInA));
btTransform frameInB(glmToBullet(rotB), glmToBullet(pointInB));
btRigidBody* rigidBodyB = getOtherRigidBody(otherEntityID);
if (!rigidBodyB) {
HIFI_FCDEBUG_ID(physics(), repeatMessageID, "ObjectConstraintSlider::getConstraint -- no rigidBodyB");
return nullptr;
}
constraint = new btSliderConstraint(*rigidBodyA, *rigidBodyB, frameInA, frameInB, true);
} else {
// This slider is between an entity and the world-frame.
glm::quat rot = glm::rotation(DEFAULT_SLIDER_AXIS, axisInA);
btTransform frameInA(glmToBullet(rot), glmToBullet(pointInA));
constraint = new btSliderConstraint(*rigidBodyA, frameInA, true);
}
withWriteLock([&]{
_constraint = constraint;
});
// if we don't wake up rigidBodyA, we may not send the dynamicData property over the network
forceBodyNonStatic();
activateBody();
updateSlider();
return constraint;
}
void AvatarActionHold::doKinematicUpdate(float deltaTimeStep) {
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning entity";
return;
}
if (ownerEntity->getParentID() != QUuid()) {
// if the held entity has been given a parent, stop acting on it.
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
qDebug() << "AvatarActionHold::doKinematicUpdate -- no owning physics info";
return;
}
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
btRigidBody* rigidBody = motionState ? motionState->getRigidBody() : nullptr;
if (!rigidBody) {
qDebug() << "AvatarActionHold::doKinematicUpdate -- no rigidBody";
return;
}
withWriteLock([&]{
if (_previousSet &&
_positionalTarget != _previousPositionalTarget) { // don't average in a zero velocity if we get the same data
glm::vec3 oneFrameVelocity = (_positionalTarget - _previousPositionalTarget) / deltaTimeStep;
_measuredLinearVelocities[_measuredLinearVelocitiesIndex++] = oneFrameVelocity;
if (_measuredLinearVelocitiesIndex >= AvatarActionHold::velocitySmoothFrames) {
_measuredLinearVelocitiesIndex = 0;
}
}
glm::vec3 measuredLinearVelocity;
for (int i = 0; i < AvatarActionHold::velocitySmoothFrames; i++) {
// there is a bit of lag between when someone releases the trigger and when the software reacts to
// the release. we calculate the velocity from previous frames but we don't include several
// of the most recent.
//
// if _measuredLinearVelocitiesIndex is
// 0 -- ignore i of 3 4 5
// 1 -- ignore i of 4 5 0
// 2 -- ignore i of 5 0 1
// 3 -- ignore i of 0 1 2
// 4 -- ignore i of 1 2 3
// 5 -- ignore i of 2 3 4
// This code is now disabled, but I'm leaving it commented-out because I suspect it will come back.
// if ((i + 1) % AvatarActionHold::velocitySmoothFrames == _measuredLinearVelocitiesIndex ||
// (i + 2) % AvatarActionHold::velocitySmoothFrames == _measuredLinearVelocitiesIndex ||
// (i + 3) % AvatarActionHold::velocitySmoothFrames == _measuredLinearVelocitiesIndex) {
// continue;
// }
measuredLinearVelocity += _measuredLinearVelocities[i];
}
measuredLinearVelocity /= (float)(AvatarActionHold::velocitySmoothFrames
// - 3 // 3 because of the 3 we skipped, above
);
if (_kinematicSetVelocity) {
rigidBody->setLinearVelocity(glmToBullet(measuredLinearVelocity));
rigidBody->setAngularVelocity(glmToBullet(_angularVelocityTarget));
}
btTransform worldTrans = rigidBody->getWorldTransform();
worldTrans.setOrigin(glmToBullet(_positionalTarget));
worldTrans.setRotation(glmToBullet(_rotationalTarget));
rigidBody->setWorldTransform(worldTrans);
motionState->dirtyInternalKinematicChanges();
_previousPositionalTarget = _positionalTarget;
_previousRotationalTarget = _rotationalTarget;
_previousDeltaTimeStep = deltaTimeStep;
_previousSet = true;
});
forceBodyNonStatic();
activateBody(true);
}
bool ObjectActionSpring::updateArguments(QVariantMap arguments) {
glm::vec3 positionalTarget;
float linearTimeScale;
glm::quat rotationalTarget;
float angularTimeScale;
bool needUpdate = false;
bool somethingChanged = ObjectAction::updateArguments(arguments);
withReadLock([&]{
// targets are required, spring-constants are optional
bool ok = true;
positionalTarget = EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ok, false);
if (!ok) {
positionalTarget = _positionalTarget;
}
ok = true;
linearTimeScale = EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", ok, false);
if (!ok || linearTimeScale <= 0.0f) {
linearTimeScale = _linearTimeScale;
}
ok = true;
rotationalTarget = EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", ok, false);
if (!ok) {
rotationalTarget = _rotationalTarget;
}
ok = true;
angularTimeScale =
EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", ok, false);
if (!ok) {
angularTimeScale = _angularTimeScale;
}
if (somethingChanged ||
positionalTarget != _positionalTarget ||
linearTimeScale != _linearTimeScale ||
rotationalTarget != _rotationalTarget ||
angularTimeScale != _angularTimeScale) {
// something changed
needUpdate = true;
}
});
if (needUpdate) {
withWriteLock([&] {
_positionalTarget = positionalTarget;
_linearTimeScale = glm::max(MIN_TIMESCALE, glm::abs(linearTimeScale));
_rotationalTarget = rotationalTarget;
_angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale));
_active = true;
auto ownerEntity = _ownerEntity.lock();
if (ownerEntity) {
ownerEntity->setActionDataDirty(true);
ownerEntity->setActionDataNeedsTransmit(true);
}
});
activateBody();
}
return true;
}
bool AvatarActionHold::updateArguments(QVariantMap arguments) {
glm::vec3 relativePosition;
glm::quat relativeRotation;
float timeScale;
QString hand;
QUuid holderID;
bool kinematic;
bool kinematicSetVelocity;
bool needUpdate = false;
bool somethingChanged = ObjectAction::updateArguments(arguments);
withReadLock([&] {
bool ok = true;
relativePosition = EntityActionInterface::extractVec3Argument("hold", arguments, "relativePosition", ok, false);
if (!ok) {
relativePosition = _relativePosition;
}
ok = true;
relativeRotation = EntityActionInterface::extractQuatArgument("hold", arguments, "relativeRotation", ok, false);
if (!ok) {
relativeRotation = _relativeRotation;
}
ok = true;
timeScale = EntityActionInterface::extractFloatArgument("hold", arguments, "timeScale", ok, false);
if (!ok) {
timeScale = _linearTimeScale;
}
ok = true;
hand = EntityActionInterface::extractStringArgument("hold", arguments, "hand", ok, false);
if (!ok || !(hand == "left" || hand == "right")) {
hand = _hand;
}
ok = true;
auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
holderID = myAvatar->getSessionUUID();
ok = true;
kinematic = EntityActionInterface::extractBooleanArgument("hold", arguments, "kinematic", ok, false);
if (!ok) {
_kinematic = false;
}
ok = true;
kinematicSetVelocity = EntityActionInterface::extractBooleanArgument("hold", arguments,
"kinematicSetVelocity", ok, false);
if (!ok) {
_kinematicSetVelocity = false;
}
if (somethingChanged ||
relativePosition != _relativePosition ||
relativeRotation != _relativeRotation ||
timeScale != _linearTimeScale ||
hand != _hand ||
holderID != _holderID ||
kinematic != _kinematic ||
kinematicSetVelocity != _kinematicSetVelocity) {
needUpdate = true;
}
});
if (needUpdate) {
withWriteLock([&] {
_relativePosition = relativePosition;
_relativeRotation = relativeRotation;
const float MIN_TIMESCALE = 0.1f;
_linearTimeScale = glm::max(MIN_TIMESCALE, timeScale);
_angularTimeScale = _linearTimeScale;
_hand = hand;
_holderID = holderID;
_kinematic = kinematic;
_kinematicSetVelocity = kinematicSetVelocity;
_active = true;
auto ownerEntity = _ownerEntity.lock();
if (ownerEntity) {
ownerEntity->setActionDataDirty(true);
}
});
activateBody();
}
return true;
}
