void Camera::update_following(double dt) {
ActorPtr following_actor = following_actor_.lock();
if(following_actor) {
kmQuaternion actor_rotation = following_actor->absolute_rotation();
kmVec3 actor_position = following_actor->absolute_position();
kmVec3 actor_forward;
kmQuaternionGetForwardVec3RH(&actor_forward, &actor_rotation);
kmQuaternion initial_rotation;
kmQuaternionAssign(&initial_rotation, &rotation_);
float t = ((following_lag_ == 0) ? 1.0 : dt * (1.0 / following_lag_));
kmQuaternionSlerp(&rotation_, &initial_rotation, &actor_rotation, t);
kmVec3 rotated_offset;
kmQuaternionMultiplyVec3(&rotated_offset, &rotation_, &following_offset_);
//kmMat4RotationQuaternion(&new_rotation_matrix, &rotation_);
//kmVec3MultiplyMat4(&rotated_offset, &following_offset_, &new_rotation_matrix);
kmVec3Add(&position_, &rotated_offset, &actor_position);
update_from_parent();
} else {
//The actor was destroyed, so reset
following_actor_ = ActorRef();
}
}
/**< Get the axis and angle of rotation from a quaternion*/
void kmQuaternionToAxisAngle(const kmQuaternion* pIn, kmVec3* pAxis, kmScalar* pAngle)
{
kmScalar scale; /* temp vars*/
kmQuaternion tmp;
if(pIn->w > 1.0) {
kmQuaternionNormalize(&tmp, pIn);
} else {
kmQuaternionAssign(&tmp, pIn);
}
*pAngle = 2.0 * acosf(tmp.w);
scale = sqrtf(1.0 - kmSQR(tmp.w));
if (scale < kmEpsilon) { /* angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0*/
pAxis->x = 0.0f;
pAxis->y = 0.0f;
pAxis->z = 1.0f;
} else {
pAxis->x = tmp.x / scale;
pAxis->y = tmp.y / scale;
pAxis->z = tmp.z / scale;
kmVec3Normalize(pAxis, pAxis);
}
}
/**< Multiplies 2 quaternions together*/
extern
kmQuaternion* kmQuaternionMultiply(kmQuaternion* pOut,
const kmQuaternion* qu1,
const kmQuaternion* qu2)
{
kmQuaternion tmp1, tmp2;
kmQuaternionAssign(&tmp1, qu1);
kmQuaternionAssign(&tmp2, qu2);
/*Just aliasing*/
kmQuaternion* q1 = &tmp1;
kmQuaternion* q2 = &tmp2;
pOut->x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y;
pOut->y = q1->w * q2->y + q1->y * q2->w + q1->z * q2->x - q1->x * q2->z;
pOut->z = q1->w * q2->z + q1->z * q2->w + q1->x * q2->y - q1->y * q2->x;
pOut->w = q1->w * q2->w - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
return pOut;
}
