double nlerp(int n, double *xin, double *f) {
if (n == 1)
return (1.0 - xin[0]) * f[0] + xin[0] * f[1];
else
return (1.0 - xin[0]) * nlerp(n - 1, xin+1, f) +
xin[0] * nlerp(n - 1, xin+1, f + (1LL << (n - 1)));
}
float Perlin::noise( float x, float y ) const
{
int32_t X = ((int32_t)floorf(x)) & 255, Y = ((int32_t)floorf(y)) & 255;
x -= floorf(x); y -= floorf(y);
float u = fade( x ), v = fade( y );
int32_t A = mPerms[X ]+Y, AA = mPerms[A], AB = mPerms[A+1],
B = mPerms[X+1]+Y, BA = mPerms[B], BB = mPerms[B+1];
return nlerp(v, nlerp(u, grad(mPerms[AA ], x , y ),
grad(mPerms[BA ], x-1, y )),
nlerp(u, grad(mPerms[AB ], x , y-1 ),
grad(mPerms[BB ], x-1, y-1 )));
}
/////////////////////////////////////////////////////////////////////////////////////////////////
// noise
float Perlin::noise( float x ) const
{
int32_t X = ((int32_t)floorf(x)) & 255;
x -= floorf(x);
float u = fade( x );
int32_t A = mPerms[X], AA = mPerms[A], B = mPerms[X+1], BA = mPerms[B];
return nlerp( u, grad( mPerms[AA ], x ), grad( mPerms[BA], x-1 ) );
}
//-----------------------------------------------------------------------------
// LLTargetingMotion::onUpdate()
//-----------------------------------------------------------------------------
BOOL LLTargetingMotion::onUpdate(F32 time, U8* joint_mask)
{
F32 slerp_amt = LLCriticalDamp::getInterpolant(TORSO_TARGET_HALF_LIFE);
LLVector3 target;
LLVector3* lookAtPoint = (LLVector3*)mCharacter->getAnimationData("LookAtPoint");
BOOL result = TRUE;
if (!lookAtPoint)
{
return TRUE;
}
else
{
target = *lookAtPoint;
target.normVec();
}
//LLVector3 target_plane_normal = LLVector3(1.f, 0.f, 0.f) * mPelvisJoint->getWorldRotation();
//LLVector3 torso_dir = LLVector3(1.f, 0.f, 0.f) * (mTorsoJoint->getWorldRotation() * mTorsoState->getRotation());
LLVector3 skyward(0.f, 0.f, 1.f);
LLVector3 left(skyward % target);
left.normVec();
LLVector3 up(target % left);
up.normVec();
LLQuaternion target_aim_rot(target, left, up);
LLQuaternion cur_torso_rot = mTorsoJoint->getWorldRotation();
LLVector3 right_hand_at = LLVector3(0.f, -1.f, 0.f) * mRightHandJoint->getWorldRotation();
left.setVec(skyward % right_hand_at);
left.normVec();
up.setVec(right_hand_at % left);
up.normVec();
LLQuaternion right_hand_rot(right_hand_at, left, up);
LLQuaternion new_torso_rot = (cur_torso_rot * ~right_hand_rot) * target_aim_rot;
// find ideal additive rotation to make torso point in correct direction
new_torso_rot = new_torso_rot * ~cur_torso_rot;
// slerp from current additive rotation to ideal additive rotation
new_torso_rot = nlerp(slerp_amt, mTorsoState->getRotation(), new_torso_rot);
// constraint overall torso rotation
LLQuaternion total_rot = new_torso_rot * mTorsoJoint->getRotation();
total_rot.constrain(F_PI_BY_TWO * 0.8f);
new_torso_rot = total_rot * ~mTorsoJoint->getRotation();
mTorsoState->setRotation(new_torso_rot);
return result;
}
//-----------------------------------------------------------------------------
// interpolate()
//-----------------------------------------------------------------------------
void LLJointStateBlender::interpolate(F32 u)
{
// only interpolate if we have a joint state
if (!mJointStates[0])
{
return;
}
LLJoint* target_joint = mJointStates[0]->getJoint();
if (!target_joint)
{
return;
}
target_joint->setPosition(lerp(target_joint->getPosition(), mJointCache.getPosition(), u));
target_joint->setScale(lerp(target_joint->getScale(), mJointCache.getScale(), u));
target_joint->setRotation(nlerp(u, target_joint->getRotation(), mJointCache.getRotation()));
}
void Animation::getPose(float time, Pose& pose, Model& model) const
{
PROFILE_FUNCTION();
if(model.isReady())
{
int frame = (int)(time * m_fps);
frame = frame >= m_frame_count ? m_frame_count - 1 : frame;
Vec3* pos = pose.getPositions();
Quat* rot = pose.getRotations();
int off = frame * m_bone_count;
int off2 = off + m_bone_count;
float t = (time - frame / (float)m_fps) / (1.0f / m_fps);
if(frame < m_frame_count - 1)
{
for(int i = 0; i < m_bone_count; ++i)
{
Model::BoneMap::iterator iter = model.getBoneIndex(m_bones[i]);
if (iter.isValid())
{
int model_bone_index = iter.value();
lerp(m_positions[off + i], m_positions[off2 + i], &pos[model_bone_index], t);
nlerp(m_rotations[off + i], m_rotations[off2 + i], &rot[model_bone_index], t);
}
}
}
else
{
for(int i = 0; i < m_bone_count; ++i)
{
Model::BoneMap::iterator iter = model.getBoneIndex(m_bones[i]);
if (iter.isValid())
{
int model_bone_index = iter.value();
pos[model_bone_index] = m_positions[off + i];
rot[model_bone_index] = m_rotations[off + i];
}
}
}
pose.setIsRelative();
pose.computeAbsolute(model);
}
}
void LLAgentPilot::moveCamera()
{
if (!getOverrideCamera())
return;
if (mCurrentAction<mActions.count())
{
S32 start_index = llmax(mCurrentAction-1,0);
S32 end_index = mCurrentAction;
F32 t = 0.0;
F32 timedelta = mActions[end_index].mTime - mActions[start_index].mTime;
F32 tickelapsed = mTimer.getElapsedTimeF32()-mActions[start_index].mTime;
if (timedelta > 0.0)
{
t = tickelapsed/timedelta;
}
if ((t<0.0)||(t>1.0))
{
llwarns << "mCurrentAction is invalid, t = " << t << llendl;
return;
}
Action& start = mActions[start_index];
Action& end = mActions[end_index];
F32 view = lerp(start.mCameraView, end.mCameraView, t);
LLVector3 origin = lerp(start.mCameraOrigin, end.mCameraOrigin, t);
LLQuaternion start_quat(start.mCameraXAxis, start.mCameraYAxis, start.mCameraZAxis);
LLQuaternion end_quat(end.mCameraXAxis, end.mCameraYAxis, end.mCameraZAxis);
LLQuaternion quat = nlerp(t, start_quat, end_quat);
LLMatrix3 mat(quat);
LLViewerCamera::getInstance()->setView(view);
LLViewerCamera::getInstance()->setOrigin(origin);
LLViewerCamera::getInstance()->mXAxis = LLVector3(mat.mMatrix[0]);
LLViewerCamera::getInstance()->mYAxis = LLVector3(mat.mMatrix[1]);
LLViewerCamera::getInstance()->mZAxis = LLVector3(mat.mMatrix[2]);
}
}
float Perlin::noise( float x, float y, float z ) const
{
// These floors need to remain that due to behavior with negatives.
int32_t X = ((int32_t)floorf(x)) & 255, Y = ((int32_t)floorf(y)) & 255, Z = ((int32_t)floorf(z)) & 255;
x -= floorf(x); y -= floorf(y); z -= floorf(z);
float u = fade(x), v = fade(y), w = fade(z);
int32_t A = mPerms[X ]+Y, AA = mPerms[A]+Z, AB = mPerms[A+1]+Z,
B = mPerms[X+1]+Y, BA = mPerms[B]+Z, BB = mPerms[B+1]+Z;
float a = grad(mPerms[AA ], x , y , z );
float b = grad(mPerms[BA ], x-1, y , z );
float c = grad(mPerms[AB ], x , y-1, z );
float d = grad(mPerms[BB ], x-1, y-1, z );
float e = grad(mPerms[AA+1], x , y , z-1 );
float f = grad(mPerms[BA+1], x-1, y , z-1 );
float g = grad(mPerms[AB+1], x , y-1, z-1 );
float h = grad(mPerms[BB+1], x-1, y-1, z-1 );
return nlerp(w, nlerp( v, nlerp( u, a, b ),
nlerp( u, c, d ) ),
nlerp(v, nlerp( u, e, f ),
nlerp( u, g, h ) ) );
}
// -----------------------------------------------------------------------------
void LLViewerJoystick::moveFlycam(bool reset)
{
static LLQuaternion sFlycamRotation;
static LLVector3 sFlycamPosition;
static F32 sFlycamZoom;
if (!gFocusMgr.getAppHasFocus() || mDriverState != JDS_INITIALIZED
|| !gSavedSettings.getBOOL("JoystickEnabled") || !gSavedSettings.getBOOL("JoystickFlycamEnabled"))
{
return;
}
S32 axis[] =
{
gSavedSettings.getS32("JoystickAxis0"),
gSavedSettings.getS32("JoystickAxis1"),
gSavedSettings.getS32("JoystickAxis2"),
gSavedSettings.getS32("JoystickAxis3"),
gSavedSettings.getS32("JoystickAxis4"),
gSavedSettings.getS32("JoystickAxis5"),
gSavedSettings.getS32("JoystickAxis6")
};
bool in_build_mode = LLToolMgr::getInstance()->inBuildMode();
if (reset || mResetFlag)
{
sFlycamPosition = LLViewerCamera::getInstance()->getOrigin();
sFlycamRotation = LLViewerCamera::getInstance()->getQuaternion();
sFlycamZoom = LLViewerCamera::getInstance()->getView();
resetDeltas(axis);
return;
}
F32 axis_scale[] =
{
gSavedSettings.getF32("FlycamAxisScale0"),
gSavedSettings.getF32("FlycamAxisScale1"),
gSavedSettings.getF32("FlycamAxisScale2"),
gSavedSettings.getF32("FlycamAxisScale3"),
gSavedSettings.getF32("FlycamAxisScale4"),
gSavedSettings.getF32("FlycamAxisScale5"),
gSavedSettings.getF32("FlycamAxisScale6")
};
F32 dead_zone[] =
{
gSavedSettings.getF32("FlycamAxisDeadZone0"),
gSavedSettings.getF32("FlycamAxisDeadZone1"),
gSavedSettings.getF32("FlycamAxisDeadZone2"),
gSavedSettings.getF32("FlycamAxisDeadZone3"),
gSavedSettings.getF32("FlycamAxisDeadZone4"),
gSavedSettings.getF32("FlycamAxisDeadZone5"),
gSavedSettings.getF32("FlycamAxisDeadZone6")
};
F32 time = gFrameIntervalSeconds;
// avoid making ridiculously big movements if there's a big drop in fps
if (time > .2f)
{
time = .2f;
}
F32 cur_delta[7];
F32 feather = gSavedSettings.getF32("FlycamFeathering");
bool absolute = gSavedSettings.getBOOL("Cursor3D");
bool is_zero = true;
for (U32 i = 0; i < 7; i++)
{
cur_delta[i] = -getJoystickAxis(axis[i]);
F32 tmp = cur_delta[i];
if (absolute)
{
cur_delta[i] = cur_delta[i] - sLastDelta[i];
}
sLastDelta[i] = tmp;
if (cur_delta[i] > 0)
{
cur_delta[i] = llmax(cur_delta[i]-dead_zone[i], 0.f);
}
else
{
cur_delta[i] = llmin(cur_delta[i]+dead_zone[i], 0.f);
}
// we need smaller camera movements in build mode
// NOTE: this needs to remain after the deadzone calculation, otherwise
// we have issues with flycam "jumping" when the build dialog is opened/closed -Nyx
if (in_build_mode)
{
if (i == X_I || i == Y_I || i == Z_I)
{
cur_delta[i] /= BUILDMODE_FLYCAM_T_SCALE;
}
}
cur_delta[i] *= axis_scale[i];
if (!absolute)
{
cur_delta[i] *= time;
}
sDelta[i] = sDelta[i] + (cur_delta[i]-sDelta[i])*time*feather;
is_zero = is_zero && (cur_delta[i] == 0.f);
}
// Clear AFK state if moved beyond the deadzone
if (!is_zero && gAwayTimer.getElapsedTimeF32() > MIN_AFK_TIME)
{
gAgent.clearAFK();
}
sFlycamPosition += LLVector3(sDelta) * sFlycamRotation;
LLMatrix3 rot_mat(sDelta[3], sDelta[4], sDelta[5]);
sFlycamRotation = LLQuaternion(rot_mat)*sFlycamRotation;
if (gSavedSettings.getBOOL("AutoLeveling"))
{
LLMatrix3 level(sFlycamRotation);
LLVector3 x = LLVector3(level.mMatrix[0]);
LLVector3 y = LLVector3(level.mMatrix[1]);
LLVector3 z = LLVector3(level.mMatrix[2]);
y.mV[2] = 0.f;
y.normVec();
level.setRows(x,y,z);
level.orthogonalize();
LLQuaternion quat(level);
sFlycamRotation = nlerp(llmin(feather*time,1.f), sFlycamRotation, quat);
}
if (gSavedSettings.getBOOL("ZoomDirect"))
{
sFlycamZoom = sLastDelta[6]*axis_scale[6]+dead_zone[6];
}
else
{
sFlycamZoom += sDelta[6];
}
LLMatrix3 mat(sFlycamRotation);
LLViewerCamera::getInstance()->setView(sFlycamZoom);
LLViewerCamera::getInstance()->setOrigin(sFlycamPosition);
LLViewerCamera::getInstance()->mXAxis = LLVector3(mat.mMatrix[0]);
LLViewerCamera::getInstance()->mYAxis = LLVector3(mat.mMatrix[1]);
LLViewerCamera::getInstance()->mZAxis = LLVector3(mat.mMatrix[2]);
}
// Returns "distance" between target destination and resulting xfrom
F32 LLDrawable::updateXform(BOOL undamped)
{
BOOL damped = !undamped;
// Position
LLVector3 old_pos(mXform.getPosition());
LLVector3 target_pos;
if (mXform.isRoot())
{
// get root position in your agent's region
target_pos = mVObjp->getPositionAgent();
}
else
{
// parent-relative position
target_pos = mVObjp->getPosition();
}
// Rotation
LLQuaternion old_rot(mXform.getRotation());
LLQuaternion target_rot = mVObjp->getRotation();
//scaling
LLVector3 target_scale = mVObjp->getScale();
LLVector3 old_scale = mCurrentScale;
LLVector3 dest_scale = target_scale;
// Damping
F32 dist_squared = 0.f;
F32 camdist2 = (mDistanceWRTCamera * mDistanceWRTCamera);
if (damped && isVisible())
{
F32 lerp_amt = llclamp(LLCriticalDamp::getInterpolant(OBJECT_DAMPING_TIME_CONSTANT), 0.f, 1.f);
LLVector3 new_pos = lerp(old_pos, target_pos, lerp_amt);
dist_squared = dist_vec_squared(new_pos, target_pos);
LLQuaternion new_rot = nlerp(lerp_amt, old_rot, target_rot);
dist_squared += (1.f - dot(new_rot, target_rot)) * 10.f;
LLVector3 new_scale = lerp(old_scale, target_scale, lerp_amt);
dist_squared += dist_vec_squared(new_scale, target_scale);
if ((dist_squared >= MIN_INTERPOLATE_DISTANCE_SQUARED * camdist2) &&
(dist_squared <= MAX_INTERPOLATE_DISTANCE_SQUARED))
{
// interpolate
target_pos = new_pos;
target_rot = new_rot;
target_scale = new_scale;
}
else
{
// snap to final position
dist_squared = 0.0f;
if (getVOVolume() && !isRoot())
{ //child prim snapping to some position, needs a rebuild
gPipeline.markRebuild(this, LLDrawable::REBUILD_POSITION, TRUE);
}
}
}
if ((mCurrentScale != target_scale) ||
(!isRoot() &&
(dist_squared >= MIN_INTERPOLATE_DISTANCE_SQUARED ||
!mVObjp->getAngularVelocity().isExactlyZero() ||
target_pos != mXform.getPosition() ||
target_rot != mXform.getRotation())))
{ //child prim moving or scale change requires immediate rebuild
gPipeline.markRebuild(this, LLDrawable::REBUILD_POSITION, TRUE);
}
else if (!getVOVolume() && !isAvatar())
{
movePartition();
}
// Update
mXform.setPosition(target_pos);
mXform.setRotation(target_rot);
mXform.setScale(LLVector3(1,1,1)); //no scale in drawable transforms (IT'S A RULE!)
mXform.updateMatrix();
mCurrentScale = target_scale;
if (mSpatialBridge)
{
gPipeline.markMoved(mSpatialBridge, FALSE);
}
return dist_squared;
}
//-----------------------------------------------------------------------------
// blendJointStates()
//-----------------------------------------------------------------------------
void LLJointStateBlender::blendJointStates(BOOL apply_now)
{
// we need at least one joint to blend
// if there is one, it will be in slot zero according to insertion logic
// instead of resetting joint state to default, just leave it unchanged from last frame
if (mJointStates[0].isNull())
{
return;
}
LLJoint* target_joint = apply_now ? mJointStates[0]->getJoint() : &mJointCache;
const S32 POS_WEIGHT = 0;
const S32 ROT_WEIGHT = 1;
const S32 SCALE_WEIGHT = 2;
F32 sum_weights[3];
U32 sum_usage = 0;
LLVector3 blended_pos = target_joint->getPosition();
LLQuaternion blended_rot = target_joint->getRotation();
LLVector3 blended_scale = target_joint->getScale();
LLVector3 added_pos;
LLQuaternion added_rot;
LLVector3 added_scale;
//S32 joint_state_index;
sum_weights[POS_WEIGHT] = 0.f;
sum_weights[ROT_WEIGHT] = 0.f;
sum_weights[SCALE_WEIGHT] = 0.f;
for(S32 joint_state_index = 0;
joint_state_index < JSB_NUM_JOINT_STATES && mJointStates[joint_state_index].notNull();
joint_state_index++)
{
LLJointState* jsp = mJointStates[joint_state_index];
U32 current_usage = jsp->getUsage();
F32 current_weight = jsp->getWeight();
if (current_weight == 0.f)
{
continue;
}
if (mAdditiveBlends[joint_state_index])
{
if(current_usage & LLJointState::POS)
{
F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);
// add in pos for this jointstate modulated by weight
added_pos += jsp->getPosition() * (new_weight_sum - sum_weights[POS_WEIGHT]);
}
if(current_usage & LLJointState::SCALE)
{
F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);
// add in scale for this jointstate modulated by weight
added_scale += jsp->getScale() * (new_weight_sum - sum_weights[SCALE_WEIGHT]);
}
if (current_usage & LLJointState::ROT)
{
F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);
// add in rotation for this jointstate modulated by weight
added_rot = nlerp((new_weight_sum - sum_weights[ROT_WEIGHT]), added_rot, jsp->getRotation()) * added_rot;
}
}
else
{
// blend two jointstates together
// blend position
if(current_usage & LLJointState::POS)
{
if(sum_usage & LLJointState::POS)
{
F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);
// blend positions from both
blended_pos = lerp(jsp->getPosition(), blended_pos, sum_weights[POS_WEIGHT] / new_weight_sum);
sum_weights[POS_WEIGHT] = new_weight_sum;
}
else
{
// copy position from current
blended_pos = jsp->getPosition();
sum_weights[POS_WEIGHT] = current_weight;
}
}
// now do scale
if(current_usage & LLJointState::SCALE)
{
if(sum_usage & LLJointState::SCALE)
{
F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);
// blend scales from both
blended_scale = lerp(jsp->getScale(), blended_scale, sum_weights[SCALE_WEIGHT] / new_weight_sum);
sum_weights[SCALE_WEIGHT] = new_weight_sum;
}
else
{
// copy scale from current
blended_scale = jsp->getScale();
sum_weights[SCALE_WEIGHT] = current_weight;
}
}
// rotation
if (current_usage & LLJointState::ROT)
{
if(sum_usage & LLJointState::ROT)
{
F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);
// blend rotations from both
blended_rot = nlerp(sum_weights[ROT_WEIGHT] / new_weight_sum, jsp->getRotation(), blended_rot);
sum_weights[ROT_WEIGHT] = new_weight_sum;
}
else
{
// copy rotation from current
blended_rot = jsp->getRotation();
sum_weights[ROT_WEIGHT] = current_weight;
}
}
// update resulting usage mask
sum_usage = sum_usage | current_usage;
}
}
if (!added_scale.isFinite())
{
added_scale.clearVec();
}
if (!blended_scale.isFinite())
{
blended_scale.setVec(1,1,1);
}
// apply transforms
target_joint->setPosition(blended_pos + added_pos);
target_joint->setScale(blended_scale + added_scale);
target_joint->setRotation(added_rot * blended_rot);
if (apply_now)
{
// now clear joint states
for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
{
mJointStates[i] = NULL;
}
}
}
//-----------------------------------------------------------------------------
// LLHeadRotMotion::onUpdate()
//-----------------------------------------------------------------------------
BOOL LLHeadRotMotion::onUpdate(F32 time, U8* joint_mask)
{
LLQuaternion targetHeadRotWorld;
LLQuaternion currentRootRotWorld = mRootJoint->getWorldRotation();
LLQuaternion currentInvRootRotWorld = ~currentRootRotWorld;
F32 head_slerp_amt = LLCriticalDamp::getInterpolant(HEAD_LOOKAT_LAG_HALF_LIFE);
F32 torso_slerp_amt = LLCriticalDamp::getInterpolant(TORSO_LOOKAT_LAG_HALF_LIFE);
LLVector3* targetPos = (LLVector3*)mCharacter->getAnimationData("LookAtPoint");
if (targetPos)
{
LLVector3 headLookAt = *targetPos;
// llinfos << "Look At: " << headLookAt + mHeadJoint->getWorldPosition() << llendl;
F32 lookatDistance = headLookAt.normVec();
if (lookatDistance < MIN_HEAD_LOOKAT_DISTANCE)
{
targetHeadRotWorld = mPelvisJoint->getWorldRotation();
}
else
{
LLVector3 root_up = LLVector3(0.f, 0.f, 1.f) * currentRootRotWorld;
LLVector3 left(root_up % headLookAt);
// if look_at has zero length, fail
// if look_at and skyward are parallel, fail
//
// Test both of these conditions with a cross product.
if (left.magVecSquared() < 0.15f)
{
LLVector3 root_at = LLVector3(1.f, 0.f, 0.f) * currentRootRotWorld;
root_at.mV[VZ] = 0.f;
root_at.normVec();
headLookAt = lerp(headLookAt, root_at, 0.4f);
headLookAt.normVec();
left = root_up % headLookAt;
}
// Make sure look_at and skyward and not parallel
// and neither are zero length
LLVector3 up(headLookAt % left);
targetHeadRotWorld = LLQuaternion(headLookAt, left, up);
}
}
else
{
targetHeadRotWorld = currentRootRotWorld;
}
LLQuaternion head_rot_local = targetHeadRotWorld * currentInvRootRotWorld;
head_rot_local.constrain(HEAD_ROTATION_CONSTRAINT);
// set final torso rotation
// Set torso target rotation such that it lags behind the head rotation
// by a fixed amount.
LLQuaternion torso_rot_local = nlerp(TORSO_LAG, LLQuaternion::DEFAULT, head_rot_local );
mTorsoState->setRotation( nlerp(torso_slerp_amt, mTorsoState->getRotation(), torso_rot_local) );
head_rot_local = nlerp(head_slerp_amt, mLastHeadRot, head_rot_local);
mLastHeadRot = head_rot_local;
// Set the head rotation.
if (mNeckState->getJoint() && mNeckState->getJoint()->getParent())
{
LLQuaternion torsoRotLocal = mNeckState->getJoint()->getParent()->getWorldRotation() * currentInvRootRotWorld;
head_rot_local = head_rot_local * ~torsoRotLocal;
mNeckState->setRotation(nlerp(NECK_LAG, LLQuaternion::DEFAULT, head_rot_local));
mHeadState->setRotation(nlerp(1.f - NECK_LAG, LLQuaternion::DEFAULT, head_rot_local));
}
return TRUE;
}
// Returns "distance" between target destination and resulting xfrom
F32 LLDrawable::updateXform(BOOL undamped)
{
BOOL damped = !undamped;
// Position
LLVector3 old_pos(mXform.getPosition());
LLVector3 target_pos;
if (mXform.isRoot())
{
// get root position in your agent's region
target_pos = mVObjp->getPositionAgent();
}
else
{
// parent-relative position
target_pos = mVObjp->getPosition();
}
// Rotation
LLQuaternion old_rot(mXform.getRotation());
LLQuaternion target_rot = mVObjp->getRotation();
//scaling
LLVector3 target_scale = mVObjp->getScale();
LLVector3 old_scale = mCurrentScale;
// Damping
F32 dist_squared = 0.f;
F32 camdist2 = (mDistanceWRTCamera * mDistanceWRTCamera);
if (damped && isVisible())
{
F32 lerp_amt = llclamp(LLCriticalDamp::getInterpolant(OBJECT_DAMPING_TIME_CONSTANT), 0.f, 1.f);
LLVector3 new_pos = lerp(old_pos, target_pos, lerp_amt);
dist_squared = dist_vec_squared(new_pos, target_pos);
LLQuaternion new_rot = nlerp(lerp_amt, old_rot, target_rot);
// FIXME: This can be negative! It is be possible for some rots to 'cancel out' pos or size changes.
dist_squared += (1.f - dot(new_rot, target_rot)) * 10.f;
LLVector3 new_scale = lerp(old_scale, target_scale, lerp_amt);
dist_squared += dist_vec_squared(new_scale, target_scale);
if ((dist_squared >= MIN_INTERPOLATE_DISTANCE_SQUARED * camdist2) &&
(dist_squared <= MAX_INTERPOLATE_DISTANCE_SQUARED))
{
// interpolate
target_pos = new_pos;
target_rot = new_rot;
target_scale = new_scale;
}
else if (mVObjp->getAngularVelocity().isExactlyZero())
{
// snap to final position (only if no target omega is applied)
dist_squared = 0.0f;
if (getVOVolume() && !isRoot())
{ //child prim snapping to some position, needs a rebuild
gPipeline.markRebuild(this, LLDrawable::REBUILD_POSITION, TRUE);
}
}
}
else
{
// The following fixes MAINT-1742 but breaks vehicles similar to MAINT-2275
// dist_squared = dist_vec_squared(old_pos, target_pos);
// The following fixes MAINT-2247 but causes MAINT-2275
//dist_squared += (1.f - dot(old_rot, target_rot)) * 10.f;
//dist_squared += dist_vec_squared(old_scale, target_scale);
}
LLVector3 vec = mCurrentScale-target_scale;
if (vec*vec > MIN_INTERPOLATE_DISTANCE_SQUARED)
{ //scale change requires immediate rebuild
mCurrentScale = target_scale;
gPipeline.markRebuild(this, LLDrawable::REBUILD_POSITION, TRUE);
}
else if (!isRoot() &&
(!mVObjp->getAngularVelocity().isExactlyZero() ||
dist_squared > 0.f))
{ //child prim moving relative to parent, tag as needing to be rendered atomically and rebuild
dist_squared = 1.f; //keep this object on the move list
if (!isState(LLDrawable::ANIMATED_CHILD))
{
setState(LLDrawable::ANIMATED_CHILD);
gPipeline.markRebuild(this, LLDrawable::REBUILD_ALL, TRUE);
mVObjp->dirtySpatialGroup();
}
}
else if (!isRoot() &&
((dist_vec_squared(old_pos, target_pos) > 0.f)
|| (1.f - dot(old_rot, target_rot)) > 0.f))
{ //fix for BUG-840, MAINT-2275, MAINT-1742, MAINT-2247
gPipeline.markRebuild(this, LLDrawable::REBUILD_POSITION, TRUE);
}
else if (!getVOVolume() && !isAvatar())
{
movePartition();
}
// Update
mXform.setPosition(target_pos);
mXform.setRotation(target_rot);
mXform.setScale(LLVector3(1,1,1)); //no scale in drawable transforms (IT'S A RULE!)
mXform.updateMatrix();
if (mSpatialBridge)
{
gPipeline.markMoved(mSpatialBridge, FALSE);
}
return dist_squared;
}
void llquat_test_object_t::test<8>()
{
F32 value1 = 15.0f;
LLQuaternion quat1(1.0f, 2.0f, 4.0f, 1.0f);
LLQuaternion quat2(4.0f, 3.0f, 6.5f, 9.7f);
LLQuaternion res_lerp, res_slerp, res_nlerp;
//test case for lerp(F32 t, const LLQuaternion &q) fn.
res_lerp = lerp(value1, quat1);
ensure("1. LLQuaternion lerp(F32 t, const LLQuaternion &q) failed",
is_approx_equal_fraction(0.181355f, res_lerp.mQ[0], 16) &&
is_approx_equal_fraction(0.362711f, res_lerp.mQ[1], 16) &&
is_approx_equal_fraction(0.725423f, res_lerp.mQ[2], 16) &&
is_approx_equal_fraction(0.556158f, res_lerp.mQ[3], 16));
//test case for lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q) fn.
res_lerp = lerp(value1, quat1, quat2);
ensure("2. LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q) failed",
is_approx_equal_fraction(0.314306f, res_lerp.mQ[0], 16) &&
is_approx_equal_fraction(0.116156f, res_lerp.mQ[1], 16) &&
is_approx_equal_fraction(0.283559f, res_lerp.mQ[2], 16) &&
is_approx_equal_fraction(0.898506f, res_lerp.mQ[3], 16));
//test case for slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b ) fn.
res_slerp = slerp(value1, quat1, quat2);
ensure("3. LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b) failed",
is_approx_equal_fraction(46.000f, res_slerp.mQ[0], 16) &&
is_approx_equal_fraction(17.00f, res_slerp.mQ[1], 16) &&
is_approx_equal_fraction(41.5f, res_slerp.mQ[2], 16) &&
is_approx_equal_fraction(131.5f, res_slerp.mQ[3], 16));
//test case for nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b) fn.
res_nlerp = nlerp(value1, quat1, quat2);
ensure("4. LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b) failed",
is_approx_equal_fraction(0.314306f, res_nlerp.mQ[0], 16) &&
is_approx_equal_fraction(0.116157f, res_nlerp.mQ[1], 16) &&
is_approx_equal_fraction(0.283559f, res_nlerp.mQ[2], 16) &&
is_approx_equal_fraction(0.898506f, res_nlerp.mQ[3], 16));
//test case for nlerp(F32 t, const LLQuaternion &q) fn.
res_slerp = slerp(value1, quat1);
ensure("5. LLQuaternion slerp(F32 t, const LLQuaternion &q) failed",
is_approx_equal_fraction(1.0f, res_slerp.mQ[0], 16) &&
is_approx_equal_fraction(2.0f, res_slerp.mQ[1], 16) &&
is_approx_equal_fraction(4.0000f, res_slerp.mQ[2], 16) &&
is_approx_equal_fraction(1.000f, res_slerp.mQ[3], 16));
LLQuaternion quat3(2.0f, 1.0f, 5.5f, 10.5f);
LLQuaternion res_nlerp1;
value1 = 100.0f;
res_nlerp1 = nlerp(value1, quat3);
ensure("6. LLQuaternion nlerp(F32 t, const LLQuaternion &q) failed",
is_approx_equal_fraction(0.268245f, res_nlerp1.mQ[0], 16) && is_approx_equal_fraction(0.134122f, res_nlerp1.mQ[1], 2) &&
is_approx_equal_fraction(0.737673f, res_nlerp1.mQ[2], 16) &&
is_approx_equal_fraction(0.604892f, res_nlerp1.mQ[3], 16));
//test case for lerp(F32 t, const LLQuaternion &q) fn.
res_lerp = lerp(value1, quat2);
ensure("7. LLQuaternion lerp(F32 t, const LLQuaternion &q) failed",
is_approx_equal_fraction(0.404867f, res_lerp.mQ[0], 16) &&
is_approx_equal_fraction(0.303650f, res_lerp.mQ[1], 16) &&
is_approx_equal_fraction(0.657909f, res_lerp.mQ[2], 16) &&
is_approx_equal_fraction(0.557704f, res_lerp.mQ[3], 16));
}