void ObjectDynamic::forceBodyNonStatic() {
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
if (motionState && motionState->getMotionType() == MOTION_TYPE_STATIC) {
ownerEntity->flagForMotionStateChange();
}
}
btRigidBody* ObjectAction::getRigidBody() {
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
return nullptr;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
return nullptr;
}
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
return motionState->getRigidBody();
}
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)));
}
}
});
}
btRigidBody* ObjectDynamic::getRigidBody() {
ObjectMotionState* motionState = nullptr;
withReadLock([&]{
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
return;
}
motionState = static_cast<ObjectMotionState*>(physicsInfo);
});
if (motionState) {
return motionState->getRigidBody();
}
return nullptr;
}
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();
}
void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) {
if (!tryLockForRead()) {
// don't risk hanging the thread running the physics simulation
qDebug() << "ObjectActionSpring::updateActionWorker lock failed";
return;
}
auto ownerEntity = _ownerEntity.lock();
if (!ownerEntity) {
return;
}
void* physicsInfo = ownerEntity->getPhysicsInfo();
if (!physicsInfo) {
unlock();
return;
}
ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
btRigidBody* rigidBody = motionState->getRigidBody();
if (!rigidBody) {
unlock();
qDebug() << "ObjectActionSpring::updateActionWorker no rigidBody";
return;
}
const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
if (_linearTimeScale < MAX_TIMESCALE) {
btVector3 offset = rigidBody->getCenterOfMassPosition() - glmToBullet(_positionalTarget);
float offsetLength = offset.length();
float speed = (offsetLength > FLT_EPSILON) ? glm::min(offsetLength / _linearTimeScale, SPRING_MAX_SPEED) : 0.0f;
// this action is aggresively critically damped and defeats the current velocity
rigidBody->setLinearVelocity((- speed / offsetLength) * offset);
}
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 angle = deltaQ.getAngle();
const float MIN_ANGLE = 1.0e-4f;
if (angle > MIN_ANGLE) {
targetVelocity = (angle / _angularTimeScale) * deltaQ.getAxis();
}
}
// this action is aggresively critically damped and defeats the current velocity
rigidBody->setAngularVelocity(targetVelocity);
}
unlock();
}
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);
}
void ObjectActionTravelOriented::updateActionWorker(btScalar deltaTimeStep) {
withReadLock([&] {
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) {
qCDebug(physics) << "ObjectActionTravelOriented::updateActionWorker no rigidBody";
return;
}
const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
if (_angularTimeScale > MAX_TIMESCALE) {
return;
}
// find normalized velocity
glm::vec3 velocity = bulletToGLM(rigidBody->getLinearVelocity());
float speed = glm::length(velocity);
const float TRAVEL_ORIENTED_TOO_SLOW = 0.001f; // meters / second
if (speed < TRAVEL_ORIENTED_TOO_SLOW) {
return;
}
glm::vec3 direction = glm::normalize(velocity);
// find current angle of "forward"
btQuaternion bodyRotation = rigidBody->getOrientation();
glm::quat orientation = bulletToGLM(bodyRotation);
glm::vec3 forwardInWorldFrame = glm::normalize(orientation * _forward);
// find the rotation that would line up direction and forward
glm::quat neededRotation = glm::rotation(forwardInWorldFrame, direction);
glm::quat rotationalTarget = neededRotation * orientation;
btVector3 targetAngularVelocity(0.0f, 0.0f, 0.0f);
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;
targetAngularVelocity = speed * deltaQ.getAxis();
if (speed > rigidBody->getAngularSleepingThreshold()) {
rigidBody->activate();
}
}
// this action is aggresively critically damped and defeats the current velocity
rigidBody->setAngularVelocity(targetAngularVelocity);
});
}
