static void simulate_trackball(quat *q, GLfloat p1x, GLfloat p1y, GLfloat p2x, GLfloat p2y) {
if (p1x == p2x && p1y == p2y) {
quat_identity(q);
}
else {
quat p1, p2, a, d;
float p1z, p2z;
float s, t;
p1z = project_to_sphere(p1x, p1y);
quat_assign(&p1, 0.0, p1x, p1y, p1z);
p2z = project_to_sphere(p2x, p2y);
quat_assign(&p2, 0.0, p2x, p2y, p2z);
quat_mul(&a, &p1, &p2);
a.w = 0.0;
s = quat_norm(&a);
quat_div_real(&a, &a, s);
quat_sub(&d, &p1, &p2);
t = quat_norm(&d) / (2.0 * R * ROOT_2_INV);
if (t > 1.0) t = 1.0;
quat_assign(q, cos(asin(t)), a.x * t, a.y * t, a.z * t);
}
}
int att_of_params(quat_t *att, double a, double b, int c) {
xyz_t v, cross;
quat_t q;
int flag=0;
v.y = a;
v.z = b;
v.x = sqrt(1 - a*a - b*b);
if (isnan(v.x)) {
fprintf(stderr,"att_of_params: %.4f^2 + %.4f^2 > 1. Normalizing and ploughing on regardless.\n",a,b);
// return 1; //invalid parameters
v.x = 0;
xyz_normalize_self(&v);
}
if (!c) {
v.x = -v.x;
if (v.x < -.9999) { // Fix singularity
printf(" singularity\n");
v.y = 1;
flag=1;
}
}
cross.x = 0;
cross.y = -v.z;
cross.z = v.y;
q.q1 = cross.x;
q.q2 = cross.y;
q.q3 = cross.z;
q.q0 = 1 + v.x;
if (quat_norm(&q) < 0.1)
printf(" q_of_params: norm was %.4f for %.3f, %.3f, %d (v.x = %.3f)\n",quat_norm(&q),a,b,c,v.x);
if (flag)
printf(" %.3f, %.3f, %d -> %.3f, %.3f, %.3f, %.3f\n",a,b,c,q.q0,q.q1,q.q2,q.q3);
quat_normalize(&q);
if (flag)
printf(" and after normalizing, that's %.3f, %.3f, %.3f, %.3f\n",q.q0,q.q1,q.q2,q.q3);
quat_inv(att,&q);
return 0;
}
static void motion(int x, int y) {
static int count = 0;
if (scaling) {
scale_factor = scale_factor * (1.0 + (((float) (begin_y - y)) / height));
begin_x = x;
begin_y = y;
glutPostRedisplay();
return;
}
else {
quat t;
simulate_trackball(&last, (2.0 * begin_x - width) / width, (height - 2.0 * begin_y) / height, (2.0 * x - width) / width, (height - 2.0 * y) / height);
begin_x = x;
begin_y = y;
quat_mul(&t, &last, &curr);
curr = t;
if (++count % 97) {
GLfloat n;
n = quat_norm(&curr);
quat_div_real(&curr, &curr, n);
}
glutPostRedisplay();
}
}
void quat_normalize(quat_t q)
/* in place */
{
size_t i;
double norm = quat_norm(q);
for(i=0; i < 4; i++)
q[i] = q[i]/norm;
}
void quat_inv(quat_t dst, quat_t q)
{
double norm2 = quat_norm(q);
norm2 = norm2*norm2;
dst[0] = q[0]/norm2;
dst[1] = -q[1]/norm2;
dst[2] = -q[2]/norm2;
dst[3] = -q[3]/norm2;
}
// q /= ||q||
void
quat_normalize(quat_t * q)
{
double norm_inv = 1.0/quat_norm(q);
q->q0 = q->q0*norm_inv;
q->q1 = q->q1*norm_inv;
q->q2 = q->q2*norm_inv;
q->q3 = q->q3*norm_inv;
}
// ===========================================================================================
void Quaternion_byAngleAndVector(quat_t *Q, const real_t q_angle, const vec3_t *q_vector)
{
vec3_t rotation_axis;
normRv(&rotation_axis, q_vector);
//real_t f = _sin(q_angle * 0.5);
real_t f, cs;
_sin_cos(q_angle*0.5, &f, &cs);
vec3_copy(&Q->v, &rotation_axis);
vec3_mult(&Q->v, f);
//Q->s = _cos(q_angle*0.5);
Q->s = cs;
quat_mult(Q, 1.0 / quat_norm(Q));
}
void Camera_offset_orientation(Camera* const camera, float yaw, float pitch) {
quat pitch_rotation, yaw_rotation;
quat_rotate(yaw_rotation, yaw, G_UP);
vec3 right;
quat_mul_vec3(right, camera->transform.orientation, G_RIGHT);
vec3_norm(right, right);
quat_rotate(pitch_rotation, pitch, right);
quat orientation;
quat_mul(orientation, yaw_rotation, pitch_rotation);
quat_mul(
camera->transform.orientation,
orientation,
camera->transform.orientation
);
quat_norm(camera->transform.orientation, camera->transform.orientation);
}
static void do_camera_physics(vector *cam_pos, quat *cam_orient)
{
vector move;
quat qturn;
/* Rotation */
camera.turn.x = CAP(turn + mouseturn_x + turn_right-turn_left, 1.0f);
camera.turn.y = CAP(pitch + mouseturn_y + pitch_up-pitch_down, 1.0f);
camera.turn.z = CAP(roll + roll_left-roll_right, 1.0f);
cam_turn_vel.x += camera.turn.x * time_diff * turnaccel;
cam_turn_vel.y += camera.turn.y * time_diff * turnaccel;
cam_turn_vel.z += camera.turn.z * time_diff * turnaccel;
cam_turn_vel.x *= powf(turndrag, -time_diff);
cam_turn_vel.y *= powf(turndrag, -time_diff);
cam_turn_vel.z *= powf(turndrag, -time_diff);
quat_make_euler(&qturn, cam_turn_vel.x*time_diff, cam_turn_vel.y*time_diff*0.75f, cam_turn_vel.z*time_diff*0.75f);
quat_mul(cam_orient, cam_orient, &qturn);
quat_norm(cam_orient, cam_orient);
/* Movement */
camera.move.x = CAP(move_x + slide_right-slide_left, 1.0f);
camera.move.y = CAP(move_y + slide_up-slide_down, 1.0f);
camera.move.z = CAP(move_z + move_forward-move_backward, 1.0f);
move = camera.move;
if (vec_mag(&move))
vec_rotate(&move, cam_orient);
cam_vel.x += move.x * time_diff * accel;
cam_vel.y += move.y * time_diff * accel;
cam_vel.z += move.z * time_diff * accel;
cam_vel.x *= powf(drag, -time_diff);
cam_vel.y *= powf(drag, -time_diff);
cam_vel.z *= powf(drag, -time_diff);
cam_pos->x += cam_vel.x * time_diff;
cam_pos->y += cam_vel.y * time_diff;
cam_pos->z += cam_vel.z * time_diff;
}
void pure_nav_attup(void)
{
int i;
double a1 = 1 / 3, a2 = 1 / 3, a3 = 1 / 3, b1 = 0.5, b2 = 0.5;
double ap1[3], ap2[3], ap3[3], ap4[3], ap5[3], ap6[3];
double k1=54.0/105.0,k2=92.0/105.0,k3=214.0/105.0;
double temp_q[4],qt[4],qtinv[4];
double q_tmp[4];
double s, beta2, sum, beta[3];
for (i = 0; i < 3; i++) // reason for floating point earth_rateor
{
ap1[i] = 0.0; ap2[i] = 0.0; ap3[i] = 0.0;
ap4[i] = 0.0; ap5[i] = 0.0; ap6[i] = 0.0;
beta[i] = 0.0;
}
for (i = 0; i < 4; i++)
{
temp_q[i] = 0.0;
q_tmp[i] = 0.0;
}
cross(p_alp1,p_alp4,ap1);
cross(p_alp1,p_alp3,ap2);
cross(p_alp2,p_alp3,ap3);
cross(p_alp3,p_alp4,ap4);
cross(p_alp2,p_alp4,ap5);
cross(p_alp1,p_alp2,ap6);
for(i=0;i<3;i++)
{
beta[i] = p_alp1[i]+p_alp2[i]+p_alp3[i]+p_alp4[i]+k1*ap1[i]+k2*(b1*ap2[i]+b2*ap5[i])+k3*(a1*ap6[i]+a2*ap3[i]+a3*ap4[i]);
}
beta2 = (pow(beta[0],2)) + (pow(beta[1],2)) + (pow(beta[2],2));
sum = 0.5 - beta2/48.0;
temp_q[0] = 1.0-beta2/8.0+pow(beta2,2)/480.0;
temp_q[1] = sum*beta[0];
temp_q[2] = sum*beta[1];
temp_q[3] = sum*beta[2];
quat_mult((double *)p_q_body2ecef,(double *)temp_q,(double *)q_tmp);
for(i=0;i< 4;i++)
{
p_q_body2ecef[i] = q_tmp[i];
}
//earth rate corr
for (i = 0; i < 3; i++)
beta[i] = 4 * del_t * omega[i];
beta2 = beta[0] * beta[0] + beta[1] * beta[1] + beta[2] * beta[2];
s = 0.5 - beta2 / 48.0;
qt[0] = 1.0 - beta2 / 8.0 + (beta2 * beta2) / 480.0;
qt[1] = s * beta[0];
qt[2] = s * beta[1];
qt[3] = s * beta[2];
quat_inv(qt,qtinv);
quat_mult(qtinv, p_q_body2ecef, q_tmp);
for (i = 0; i < 4; i++)
p_q_body2ecef[i] = q_tmp[i];
quat_norm((double *)p_q_body2ecef);
} //end of nav_attup
/*
==============
RE_BuildSkeleton
==============
*/
int RE_BuildSkeleton(refSkeleton_t *skel, qhandle_t hAnim, int startFrame, int endFrame, float frac, qboolean clearOrigin)
{
skelAnimation_t *skelAnim;
skelAnim = R_GetAnimationByHandle(hAnim);
if (skelAnim->type == AT_MD5 && skelAnim->md5)
{
int i;
md5Animation_t *anim = skelAnim->md5;
md5Channel_t *channel;
md5Frame_t *newFrame, *oldFrame;
vec3_t newOrigin, oldOrigin, lerpedOrigin;
quat_t newQuat, oldQuat, lerpedQuat;
int componentsApplied;
// Validate the frames so there is no chance of a crash.
// This will write directly into the entity structure, so
// when the surfaces are rendered, they don't need to be
// range checked again.
//if((startFrame >= anim->numFrames) || (startFrame < 0) || (endFrame >= anim->numFrames) || (endFrame < 0))
//{
// Ren_Developer( "RE_BuildSkeleton: no such frame %d to %d for '%s'\n", startFrame, endFrame, anim->name);
// //startFrame = 0;
// //endFrame = 0;
//}
Q_clamp(startFrame, 0, anim->numFrames - 1);
Q_clamp(endFrame, 0, anim->numFrames - 1);
// compute frame pointers
oldFrame = &anim->frames[startFrame];
newFrame = &anim->frames[endFrame];
// calculate a bounding box in the current coordinate system
for (i = 0; i < 3; i++)
{
skel->bounds[0][i] =
oldFrame->bounds[0][i] < newFrame->bounds[0][i] ? oldFrame->bounds[0][i] : newFrame->bounds[0][i];
skel->bounds[1][i] =
oldFrame->bounds[1][i] > newFrame->bounds[1][i] ? oldFrame->bounds[1][i] : newFrame->bounds[1][i];
}
for (i = 0, channel = anim->channels; i < anim->numChannels; i++, channel++)
{
// set baseframe values
VectorCopy(channel->baseOrigin, newOrigin);
VectorCopy(channel->baseOrigin, oldOrigin);
quat_copy(channel->baseQuat, newQuat);
quat_copy(channel->baseQuat, oldQuat);
componentsApplied = 0;
// update tranlation bits
if (channel->componentsBits & COMPONENT_BIT_TX)
{
oldOrigin[0] = oldFrame->components[channel->componentsOffset + componentsApplied];
newOrigin[0] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
if (channel->componentsBits & COMPONENT_BIT_TY)
{
oldOrigin[1] = oldFrame->components[channel->componentsOffset + componentsApplied];
newOrigin[1] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
if (channel->componentsBits & COMPONENT_BIT_TZ)
{
oldOrigin[2] = oldFrame->components[channel->componentsOffset + componentsApplied];
newOrigin[2] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
// update quaternion rotation bits
if (channel->componentsBits & COMPONENT_BIT_QX)
{
((vec_t *) oldQuat)[0] = oldFrame->components[channel->componentsOffset + componentsApplied];
((vec_t *) newQuat)[0] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
if (channel->componentsBits & COMPONENT_BIT_QY)
{
((vec_t *) oldQuat)[1] = oldFrame->components[channel->componentsOffset + componentsApplied];
((vec_t *) newQuat)[1] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
if (channel->componentsBits & COMPONENT_BIT_QZ)
{
((vec_t *) oldQuat)[2] = oldFrame->components[channel->componentsOffset + componentsApplied];
((vec_t *) newQuat)[2] = newFrame->components[channel->componentsOffset + componentsApplied];
}
QuatCalcW(oldQuat);
quat_norm(oldQuat);
QuatCalcW(newQuat);
quat_norm(newQuat);
#if 1
VectorLerp(oldOrigin, newOrigin, frac, lerpedOrigin);
quat_slerp(oldQuat, newQuat, frac, lerpedQuat);
#else
VectorCopy(newOrigin, lerpedOrigin);
quat_copy(newQuat, lerpedQuat);
#endif
// copy lerped information to the bone + extra data
skel->bones[i].parentIndex = channel->parentIndex;
if (channel->parentIndex < 0 && clearOrigin)
{
VectorClear(skel->bones[i].origin);
QuatClear(skel->bones[i].rotation);
// move bounding box back
VectorSubtract(skel->bounds[0], lerpedOrigin, skel->bounds[0]);
VectorSubtract(skel->bounds[1], lerpedOrigin, skel->bounds[1]);
}
else
{
VectorCopy(lerpedOrigin, skel->bones[i].origin);
}
quat_copy(lerpedQuat, skel->bones[i].rotation);
#if defined(REFBONE_NAMES)
Q_strncpyz(skel->bones[i].name, channel->name, sizeof(skel->bones[i].name));
#endif
}
skel->numBones = anim->numChannels;
skel->type = SK_RELATIVE;
return qtrue;
}
else if (skelAnim->type == AT_PSA && skelAnim->psa)
{
int i;
psaAnimation_t *anim = skelAnim->psa;
axAnimationKey_t *newKey, *oldKey;
axReferenceBone_t *refBone;
vec3_t newOrigin, oldOrigin, lerpedOrigin;
quat_t newQuat, oldQuat, lerpedQuat;
refSkeleton_t skeleton;
Q_clamp(startFrame, 0, anim->info.numRawFrames - 1);
Q_clamp(endFrame, 0, anim->info.numRawFrames - 1);
ClearBounds(skel->bounds[0], skel->bounds[1]);
skel->numBones = anim->info.numBones;
for (i = 0, refBone = anim->bones; i < anim->info.numBones; i++, refBone++)
{
oldKey = &anim->keys[startFrame * anim->info.numBones + i];
newKey = &anim->keys[endFrame * anim->info.numBones + i];
VectorCopy(newKey->position, newOrigin);
VectorCopy(oldKey->position, oldOrigin);
quat_copy(newKey->quat, newQuat);
quat_copy(oldKey->quat, oldQuat);
//QuatCalcW(oldQuat);
//QuatNormalize(oldQuat);
//QuatCalcW(newQuat);
//QuatNormalize(newQuat);
VectorLerp(oldOrigin, newOrigin, frac, lerpedOrigin);
quat_slerp(oldQuat, newQuat, frac, lerpedQuat);
// copy lerped information to the bone + extra data
skel->bones[i].parentIndex = refBone->parentIndex;
if (refBone->parentIndex < 0 && clearOrigin)
{
VectorClear(skel->bones[i].origin);
QuatClear(skel->bones[i].rotation);
// move bounding box back
VectorSubtract(skel->bounds[0], lerpedOrigin, skel->bounds[0]);
VectorSubtract(skel->bounds[1], lerpedOrigin, skel->bounds[1]);
}
else
{
VectorCopy(lerpedOrigin, skel->bones[i].origin);
}
quat_copy(lerpedQuat, skel->bones[i].rotation);
#if defined(REFBONE_NAMES)
Q_strncpyz(skel->bones[i].name, refBone->name, sizeof(skel->bones[i].name));
#endif
// calculate absolute values for the bounding box approximation
VectorCopy(skel->bones[i].origin, skeleton.bones[i].origin);
quat_copy(skel->bones[i].rotation, skeleton.bones[i].rotation);
if (refBone->parentIndex >= 0)
{
vec3_t rotated;
quat_t quat;
refBone_t *bone = &skeleton.bones[i];
refBone_t *parent = &skeleton.bones[refBone->parentIndex];
QuatTransformVector(parent->rotation, bone->origin, rotated);
VectorAdd(parent->origin, rotated, bone->origin);
QuatMultiply1(parent->rotation, bone->rotation, quat);
quat_copy(quat, bone->rotation);
AddPointToBounds(bone->origin, skel->bounds[0], skel->bounds[1]);
}
}
skel->numBones = anim->info.numBones;
skel->type = SK_RELATIVE;
return qtrue;
}
//Ren_Warning( "RE_BuildSkeleton: bad animation '%s' with handle %i\n", anim->name, hAnim);
// FIXME: clear existing bones and bounds?
return qfalse;
}
